AN INTRODUCTION 
TO ROBOTICS: 
MECHANICAL ASPECTS 


Pierre DUYSINX 
and 


Michel GERADIN 


UNIVERSITY OF LIEGE 
NOVEMBRE 2004 


Contents 


1 INTRODUCTION 1 
1.1 Origin of the name robot ................-.^..... eA 2 
1.2 Mechanical role of a robot manipulator. ....................00. 2 
1.3 General structure of a robot manipulator ....................4. 3 
1.4 Structure of the control unit... 2... 2.0.0.0. 0 2.000000 en 4 
1.5 Industrial robots at the present day .............. 0.000000 22s 4 
1.6 The mechanical aspects of robotics ....... ees 5 
1.7  Multidisciplinary aspects of robotics . . .. 2222s 6 

2 BASICS OF ROBOTICS TECHNOLOGY 1 
2.1 The Mechanical Structure of a Robot. ....... 2e 2 

2.1.1 Degrees of freedom of a rigid body ........... llle 2 
2.1.2 Joints and kinematic constraints . . . ..... a 2 
2.1.3 Generalized coordinates .......... 2e 3 
2:2. Kinematic: Paige s e 52229 kx M pe MMO EY eee $T. 3 
2.2.1. Number of degrees of freedom of the joint ....... llle. 3 
2:2.2- Classification of joints: &.:« 6 eu E tw RUE Rm OX Ree dud 3 
2:2:3- Higher and lower pairs e- su «wx o RUE S Es 5 
2.24 Graphic representation of joints . . . 2222s 5 
2.2.5 Joints used inrobots s a e a 22e 5 
2.3 Topology of Kinematic Chains ........... e 8 
2.3.1 Classification of robot topologies ........... llle 8 
2.3.2 Description of simple open-tree structures . ...... ls. 11 
2.4 Mobility Index and Number of dof for a Simple Open- Tree Manipulator . .... 11 
2.4.1 Mobility index and GRÜBLER formula ...... ess 11 
2.4.2 Number of dof of a simple open-tree structure. ............... 13 
2.4.3 Number of DOF ofa manipulator ............... sns 13 
2.44. Joint Space: eo uw od egest em UA Ae a a a e 13 
2:24:55. "Las SDACG +z: case mode uum CE Re nre ees ER Reb. nb t 13 
2.16. Redundancy ni 24 EGO X ese UR RR a a ae eae Ba A 13 
247. “Singularity? z 3:3 ho ble Qux Eee a Ie xu XO Rohde oe 14 
248 Examples 3433 dco evi Be ense eode gr A 16 
DAL? “EXELCICES asperi ORA Puede REC WE Neuer d dod a a 18 
2.5 Number of dof of the task ........ 2s 20 
2.6 “Robot-morphology: xo SS a Ba A DA Aaa 22 
2.6.1 Number of possible morphologies ...................200.- 22 
2.6.2 General structure of a manipulator. .................004. 22 
2.6.3 Possible arm architectures . . .... 22e 23 


ii 


2:7 


2.8 


2.9 


CONTENTS 


2.6.4 Kinematic decoupling between effector orientation and position ...... 23 
Workspace of a robot manipulator ............ 2e 25 
2h Definitions goes RR SERE S EE ga pun a 25 
2.7.2 Comparison of the workspaces with different arm configurations . .... 25 
2.7.3 Workspace optimisation . . . . . les 26 
Accuracy, repeatability and resolution ............. les 32 
2:3;], Static accHEACy v Reve eR e oboe ALAS RUE RR fme 32 
2:82. Repeatability. 23 6060944 RPPE ARR USE 32 
2:819. “Resolutions «225i RAE STR Rar ege 9e Wes 32 
28:4: Normalisation zi EX YS UM ag Ee EOE OR d eS 33 
Robot ACtUATOES ED ee ese ee Ad ee a a es a 33 
2.9.1 Distributed motorization. . .. . 2.2.0.0. 0000 ee ee 33 
2.9.2 Centralized motorization. . .. .. 22e 34 
2:9:9; Mixed motorization> aa imer RA AME BO EIE a E aS 35 
Mechanical characteristics of actuators . . . . 22e 35 
2.10.1 Power to mass ratio . . . 4... les 35 
2.10.2 Maximum acceleration and mechanical impedance adaptation . . . . . .. 36 
Different types of actuators . .. . eh 41 
2:ILE Step Motor et wer cR Rede er Rm mm eR SEU RR 41 
2.11.2- Diréct.cürrent Motors’ .. 2 2 mo ee ee À 42 
2.11.3. -Hydraülic actuators: 495639: hob eS at quoted od Eg ws 42 
‘Phe Sensors" 4.44.22 2g Se Ara aera a ake BP RASA Lee a4, 43 
Integration of sensors in the mechanical structure... ............0.00. 43 
2.13.1 -Positions€nsors:s 4.64444 ¢ R4 wm) ba Be Eee eee EE EG 44 
2.13.2 Velocity sensors. . .. 22e 4T 
The robot manipulator ASEA-IRb-6 ...........2 en 51 
Technical sheets of some industrial robot manipulators ............... 57 
2.15.1 Industrial robot ASEA IRb-6/2................0. 000005 58 
2.15.2 Industrial robot SCEMI6P-01 ....................0048. 61 
2.15.3 Industrial robot PUMA 560... .... eA 63 
2.15.4 Industrial robot ASEA IRBI400 ....... se 65 


BASIC PRINCIPLES OF ROBOT MOTION CONTROL 


3.1 


3.2 
3.3 


3.4 


3.5 
3.6 


1 
Objectives of robot control ... eA 2 
3.1.1 Variables under control .......... les 2 
3.1.2 Robot motion control ........... eA 3 
3.1.3 Level 1 of control: artificial intelligence level... ..... aaa 5 
3.1.4 Level 2 of control or the control mode level ............... s. 5 
3.1.5 Level3 of control or servo-system level... ..... llle 6 


Kinematic model of a robot manipulator .. . ........ lll 7 
Trajectory planning 4.4 mase mom Re Eat Ee Pe a cR 11 
dol. Joint space description v « a acces ADR SOE PS V 12 
3.3.2 Cartesian space description ......... eee ee 12 
3.3.3 Description of operational motion. . .... les 13 
Dynamic model of a robot manipulator .................. cns 14 
3.4.1 The concept of dynamic model and its role ................. 14 
3.4.2 Dynamic model of a two-link manipulator .. .......... sss. 15 
Dynamic model in the general case... .. oes 18 
Dynamic control according to linear control theory ............ len 18 


3.6.1 The objective of control theory ........ eee 19 


CONTENTS 


3.7 


3.8 
3.9 
3.10 


3.11 
3.12 


3.13 


4.1 
4.2 


4.3 
4.4 
4.5 
4.6 
4.7 
4.8 


4.9 


4.10 


4.11 


4.12 


4.13 


4.14 
4.15 


4.16 


3.6.2 Open-loop equations for motion of a physical system ............ 
3.6.3 Closed-loop equation of motion . ..... 22s 
3.6.4 Stability, damping and natural frequency of the closed-loop system . . .. 
3.6.5 Position control... . . . e eA 
3:0:6- Integral correction « 2 iy enemy ehe ldo ds bua oe Re eate d 
3:0.7. - “Trajectory following: > i.c eee a x a rne mm OX RE s 
3.6.8 Control law partitioning... . . 4 4 eee 
Motion Control of Non-Linear and Time-Varying Systems ............. 
3.7.1 Design of nonlinear control laws .................... s. 
Multi-Variable Control Systems . . . 2... ee 
Multi-variable Problem Manipulators. .................... 2000. 
Practical Considerations . . . 2... lees 
3.10.1 Lack of knowledge of parameters . . .... eA 
Time Effects in Computing the Model .......... llle 
Present Industrial Robot Control Systems ......... les 
3.12.1 Individual joint PID control... ..... eA 
3.12.2 Individual joint PID control with effective joint inertia. . . . ... .... 
3.12:3- Inertial.découpling 4&4 2.154 IRR RS IP REA 
Cartesian Based Control Systems . ...... eA 
3.13.1 Comparison with joint based schemes ......... llle 


KINEMATICS OF THE RIGID BODY 


Introduction uL uu ERR RE Eee esed aperte RA uA Rd 
The rotation operator . .. . . 2 llle 
4.2.1 Properties of rotation . . . . les 
2:2:2. Remark «ud noted Ne qwe uu dii 
Position and orientation of a rigid body ............ less 
Algebraic expression of the rotation operator . . ... 22e 
The plane rotation operator . . . . . 4 lees 
Finite rotation in terms of direction cosines ........ llle 
Finite rotation in terms of dyadic products ...............2..000. 
Composition of Finite Rotations ............. 0.000002. eee 
4.8.1 Composition rule of rotations . ...... 2.020.020 000.4 
4.8.2 Non commutative character of finite rotations. ............... 
Bulemanglessct. "rcc Ge wat ee ee Sw MARS 
4.9.1]. - Singular values «s so eee bh be AE OO ee ob Rn 
4:9.2- Inversion s je a dee OS wh a ad a ee RA em mod 
Finite rotations in terms of Bryant angles ............ ee ee 
4,10:1 Singularities 4 245483. oo aes eR RR Renee nm ay banana ee wn en oh OR 
40.2. Inversión: 22: och RR Bel ee eM E ep a eS DABS 
Unique rotation about an arbitrary axis . . 1... eee 
4 lll inversione su a s: eia oa ed he X a re ela A uo BP grs 
Finite rotations in terms of Euler parameters .. .......... lll. 
Rodrigues” parameters. . 2... s Ro ome ee ec URB RADAR wo a 
Translation and angular velocities... 222A 
Explicit expression for angular velocities . . ...... 2e 
4.15.1 In terms of Euer Angles... . 2e 
4.15.2 In terms of Bryant angles .......... leer 
4.15.3 In terms of Euler parameters ....... 0000000 2G ee 
Infinitesimal displacement ..................... ee 


ii 


iv 


CONTENTS 


4-17, -Accelerations" foi pe nd ao ttu Ld Lo kso ose ropes xm iudi as tu 


4.18 Screw or helicoidal motion. ..... 2.2 le 
4.19 Homogeneous representation of vectors . . .. ele 
4.20 Homogeneous representation of frame transformations ............. 
4.21 Successive homogeneous transformations... .. eee 
4.22 Object manipulation in space . . .. . . 2 les 
4.23 Inversion of homogeneous transformation . . ...... eee 
4.24 Closed loop of homogeneous transformation . ...... eee 
4.25 Homogeneous representation of velocities... .. 22e 
4.26 Homogeneous representation of accelerations . ......... lll. 


KINEMATICS OF SIMPLY CONNECTED OPEN-TREE STRUCTURES 
5b Introduction 4... cs 2 9.3.56 oak a ee ee ES eI eek th cael dS V 
5.2 Link description by Hartenberg-Denavit method .................. 
5.3 Kinematic description of an open-tree simply connected structure. ........ 
5.3.1 Link coordinate system assignment . . .... eee. 
5.4 Geometric model of the PUMA 560... ..... 22e 
5.5 Link description using the Sheth method . . .... lens 
5.6 Sheth's geometric transformation . .... een 
5.7 Relative motion transformations . . ...... eh 
bl The revolute pal£ Hesa ay ma a eee eet 
5:5:2. Theprismatic pair ie te EO wk Re ee eee gun a 
5.593. Pheeylindrical: pair" 4 xeu Be ee wee De Aa a os A 
bu 4; “The screw joint: ae 0.4 cra we y RU AIR REGUM EE E A 
5:555". ~Thesgeark pit, uocem goncaya Gee BODE REE ONY ee 
5.8 Sheth’s description of the PUMA 560................ 2.02.22 000. 
5.9 Inversion of geometric kinematic model .............. leen 
5.10 Closed form inversion of PUMA 560robot .. ...... lens 
5.10.1 Decoupling between position and orientation ................ 
5.10.2 Pieper’s technique ........ le 
5.10.3 General procedure to determine joint angles... ...... l.l. 
510.4: Calculation: of Q1. ieies mcm IR X RA X ui ES UTE 
5.10.5 Calculation of 09 and 3 . .... a 
5.10.6 Calculation of 04, 05 and ĝe ... ooa a 
5.11 Numerical solution to inverse problem . ..... lees 
5.12 Linear mapping diagram of the jacobian matrix . . . ..... llle 
5.13 Effective computation of Jacobian matrix ............. ee 
5.14 Jacobian matrix of Puma manipulator ............. e 
5.15 Effective numerical solution of inverse problem ..................-. 
5.16 Recursive calculation of velocities in absolute coordinates .. ........... 
5.17 Recursive calculation of accelerations in absolute coordinates .. ......... 
5.18 Recursive calculation of velocities in body coordinates ............... 
5.19 Recursive calculation of accelerations in body coordinates ............. 


DYNAMICS OF OPEN-TREE SIMPLY-CONNECTED STRUCTURES 

Ol Introductions 2 a 433 of A rere A ren D ee ho det qt eoe did ud ed 

6.2 Equation of motion of the rigid body . . ...... een 
6:21" 3Potential'energy. vases een Sas piel ak SS eR haue wu ee a 
0:2:2: -I&metic energyo i uec dme RN eie E kN eau s 
6.2.83 Virtual work of external loads . . ...... eee 


CONTENTS " 


B.7.1 Example 1: Composition of rotations with quaternions........... 


6.2.4 Relationship between angular velocities and quasi-coordinates . . . . . .. 7 
6:2:5- ~“Equations-of motion: se bisque ec owe v PRA 8 

6.3 Recursive Newton-Euler technique for simple open-tree structures. . . . . .. . . 10 
6.3.1 Forward kinematic recursion . ... 22e 11 

6.3.2 Link inertia forces ...... 22e 12 
6.3.3 Force backward recursion .......... 22s 12 

6:4- Lagrangian dynamics s Ti a kita eSI ROS EAA OR Yo ede d qe 12 
6.4.1 Structure of the kinetic energy ...... eA 13 
6.4.2: Potential energy a. «spesso E YAIe vv ee 13 
6.4.3 Virtual work of external forces .......... ls 14 
6.4.4 Structure of inertia forces . . . ooo ee 14 
6:4.5- “Gravity forces e eo 40404 SUE Ro io Bye ee ea bint RU RR AUR 15 
6.4.6 Equations of motion . . . 2 les 15 

6.5 Recursive Lagrangian formulation. . .... ee 15 
6:5.T. - Forward, kinematics: ~ sg e ker eee $e ek ey gemere SPERA 15 
6:5:2> Kinetic energy era n Ru tik Ve Nee PER ed ER 16 
6.533: Matrix of inertia: PC ---—————— m 16 
6.5.4. .Botentialénergy- 24 careca eyed a d Sian ln ee We qo Ri 17 
6.5.5 Partial derivatives of kinetic energy... ... e 17 
6.5.6 Partial derivatives of potential energy .................0.. 18 
6.5.7 Equations of motion .... les 18 
6.5.8 Recursive form of equations of motion .................-.. 18 

7 TRAJECTORY GENERATION 1 
A ELEMENTS OF VECTOR AND MATRIX ALGEBRA 1 
Al Introductions uw awe AE a kh A oe ee a al 2 
A.2 Definitions and basic operations of vector calculus ..............20-. 2 
A.3 Definitions and basic operations of matrix algebra ..............0.0. 8 
A.4 Matrix representation of vector operations... . 22e 12 
B ELEMENTS OF QUATERNION ALGEBRA 1 
B: Definition «e Bo aa A ee ee Se a ee Ie 2 
B.2 representation of finite rotations in terms of quaternions .............. 3 
B.3 Matrix representation of quaternions ........... e e 00 ee eee 4 
B.4 Matrix form of finite rotations... . . 2 2 2 2 ee 4 
B.5 Angular velocities in terms of quaternions . ... e 5 
B.6 Matrix form of angular velocities ......... a 6 
Bert ‘Examples: T oe: C "mma 6 
6 

7 


B.7.2 Example 2: Using quaternions for determining robot orientation ..... 


vi 


CONTENTS 


Chapter 1 


INTRODUCTION 


2 CHAPTER 1. INTRODUCTION 


The object of the following sections is to introduce some terminology and to briefly define 
the main topics which will be covered during the course. 


1.1 Origin of the name robot 


The word robot was coined in 1920 by the Czech author K. Capek in his play R.U.R. (Rossum's 
Universal Robot), and is derived from the word robota, meaning worker. 

Later, an industrial robot has been defined as reprogrammable multifunctional manipulator, 
designed to move materials, parts, tools, or other specialized devices by means of variable 
programmed motions and to perform a variety of other tasks. In a broader context, the term 
robot also includes manipulators that are activated directly by an operator. This includes 
manipulators used in nuclear experiences, medical investigation or surgery as well as robots 
used for under-water exploration or works. 

More generally, an industrial robot has been described by the International Organization for 
Standardization (ISO) as follows: A machine formed by a mechanism including several degrees 
of freedom, often having the appearance of one or several arms ending in wrist capable of holding 
a tool, a workpiece, or an inspection device. 

In particular, its central control unit must use a memorizing device and it may sometimes use 
sensing or adaptation appliances to take into account environment and circumstances. These 
multipurpose machines are generally designed to carry out a repetitive function and can be 
adapted to other operations. 

Introduced in the early 1960s, the first industrial robots were used in hazardous operations, 
such as handling toxic and radioactive materials, loading and unloading hot workpieces from 
furnaces and handling them in foundries. Some rule-of-thumb applications for robots are the 
three D's (dull, dirty, and dangerous including demeaning but necessary tasks), and the three 
H's (hot, heavy, and hazardous). From their early uses in worker protection and safety in man- 
ufacturing plants, industrial robots have been further developed and have become important 
components in manufacturing process and systems. They have helped improve productivity, in- 
crease product quality, and reduce labor costs. Computer-controlled robots were commercialized 
in the early 1970s, with the first robot controlled by a minicomputer appearing in 1974. 


1.2 Mechanical role of a robot manipulator 


Whatever be the function assigned to it (handling, painting, assembly, welding...) a robot 
manipulator is designed mechanically to locate in space a tool often called effector. 


The effector can simply be a gripper designed to grasp a part, a painting gun, or may consist 
of any other tool. 


The geometric location of the effector is generally quite arbitrary in the reachable range, called 
workspace of the manipulator, and is continuously changing with time: it follows a path or tra- 
jectory corresponding to the specified task. At a crude but important level, it is completely 
described through a sequence of positions of one given point on the effector and its orienta- 
tion about these points. At a higher level, the trajectory planning might also take account of 
environmental constraints. 


In order to describe the position and orientation of the effector in space, it will be necessary to 
attach to it a coordinate system, or frame, and describe the position and orientation of this frame 
to some reference system. Frame transformations will thus play a fundamental role throughout 
this course. 
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Figure 1.3.1: General structure of a robot manipulator integrated in its environment 


1.9 General structure of a robot manipulator 


'To fulfill the function just described, the general structure of a robot manipulator, when con- 
sidered in its working environment, may be decomposed into five main components interacting 
together as described in figure 1.3.1. 


1. The mechanical structure, or articulated mechanism, is ideallp made of rigid members 
or links articulated together through mechanical joints. It carries at its end the tool or 
effector. 


2. The actuators provide the mechanical power in order to act on the mechanical structure 
against gravity, inertia and other external forces to modify the configuration and thus, the 
geometric location of the tool. The actuators can be of electric, hydraulic or pneumatic 
type and have to be controlled in the appropriate manner. The choice of their control 
mode is one of the fundamental options left to the mechanical engineer. 


3. The mechanical transmission devices (such as gear trains) connect and adapt the actuators 
to the mechanical structure. Their role is twofold: to transmit the mechanical efforts from 
the power sources to the mechanical joints and to adapt the actuators to their load. 


4. The sensors provide senses to the robot. They can take for example the form of tactile, op- 
tical or electrical devices. According to their function in the system they may be classified 
in two groups: 


e Proprioceptive sensors provide information about the mechanical configuration of the 
manipulator itself (such as velocity and position information); 
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e Exteroceptive sensors provide information about the environment of the robot (such 
as distance from an obstacle, contact force...) 


5. The control unit assumes simultaneously three different roles: 


e An information role, which consists of collecting and processing the information pro- 
vided by the sensors; 


e A decision role, which consists of planning the geometric motion of the manipulator 
structure starting from the task definition provided by the human operator and from 
the status of both the system and its environment transmitted by the sensors; 


e A communication role , which consists to organize the flow of information between 
the control unit, the manipulator and its environment. 


1.4 Structure of the control unit 


In order to assume the functions just described, the control unit must dispose of softwares and 
knowledge bases such as 


e a model (kinematic and/or dynamic) of the robot, which expresses the relationship between 
the input commands to the actuators and the resulting motion of the structure; 


e a model of the environment, which describes geometrically the working environment of the 
robot. It provides information such as the occurrence of zones where collisions are likely 
to occur and allows to plan the path accordingly; 


e control algorithms which govern the robot motion at a lower level and are responsible for 
the mechanical response of the structure and its actuators (assuming thus position and 
velocity control with prescribed accuracy and stability characteristics); 


e a communication protocol, which assumes the management of the messages (in shape, 
priority...) exchanged between the various components of the system. 


'The control unit may have either a centralized architecture, in which case the same processor 
assumes all the functions described above, or a hierarchical organization, in which case the 
system is organized around a master unit that assigns to each one of the slave units some of 
the functions to be performed. For example, lower level functions such as position and velocity 
control of the actuators are often assumed by slave processors. 


1.5 Industrial robots at the present day 


An industrial robot such as described above has the theoretical ability to adapt itself to any 
change on the environment on which it operates. However, most of the industrial robots available 
today, even at a research level, possess this characteristic of adaptability only to a limited extent. 
'This, because the information, decision and communication roles of the control unit which have 
been identified above are assumed only up to a very low degree. 


To improve the situation, much progress has still to be made in various areas such as sensor 
design and processing, world modelling, programming methods, decision making and system 
integration. 
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Figure 1.5.1: Schematic representation of present industrial robots 


'The present day industrial robots correspond thus to a simpler structure. In particular, their 
interaction with the environment is often almost inexistent, so that they can be regarded as 
simple robot manipulators which are programmed according to a fixed description of task (Fig- 
ure 1.5.1). 


1.6 The mechanical aspects of robotics 


Robots are required to have high mobility and dexterity in order to work in a large reachable 
range, or workspace, and access crowded spaces, handle a variety of workpieces, perform flexible 
tasks. 


It is the parallelism with the mechanical structure of the human arm which gives them these 
properties. 

Like the human arm, a robot mechanical structure is most often a purely serial linkage composed 
of cantilever beams forming a sequence of links, or members, connected by hinge joints. Such 
a structure has inherently poor mechanical stiffness and accuracy and is thus not naturally 
appropriate for heavy and/or high precision applications. 

In order to benefit from the theoretical high mobility and dexterity of the serial linkage, these 
difficulties must be overcome by advanced design, modelling and control techniques. 


'The geometry of manipulator arms, due to its serial nature, is described by complex nonlinear 
equations. Effective analytical tools are needed to construct and understand the geometric and 
kinematic model of a manipulator. Kinematics is an important area of robotics research, since 
it had traditionally focused on mechanisms with very limited number of degrees of freedom 
(generally, single input) while robotic arms have developed the need for models of multi-degree 
of freedom mechanisms. 


Dynamic behavior of robot manipulators is also a complex subject, for several reasons. 


e The kinematic complexity of articulated systems affects also the dynamic model: the 
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equations governing the dynamics of a robot arm are thus highly coupled and nonlinear; 
the motion of each joint is significantly influenced by the motion of all other joints. 


e The gravity and inertia loads applied to each member vary widely with the configuration. 
Coriolis an centrifugal effects become significant when the manipulator arm moves either 
at high speed or in a low gravity environment. 


e Structural flexibility (either concentrated in the joints or distributed over the links) greatly 
affects the dynamic behavior of the system by introducing supplementary degrees of free- 
dom of elastic nature. 


e Undesirable effects such as structural damping, friction and backlash at joints are difficult 
to model and quantify and are also responsible for significant departure from the expected 
dynamic properties of the system. 


From what preceeds, the role of the mechanical engineer in robotics appears to be important at 
least at three levels: 


e At the overall design level, he has to define the general architecture of the mechanical 
structure in order to fulfill correctly the mobility and dexterity requirements of its assigned 
function. This task requires a strong background in kinematics of mechanisms. The 
currently available computer aided design software tools are of great help to the mechanical 
engineer to compare various possible designs. 


e At the detailed design level, the role of the mechanical engineer consists to design and size 
the mechanical parts of the system in order to satisfy quality and performance criteria with 
respect to accuracy, reliability, life cycle time, mechanical strength, lightweight, operating 
speed, etc. 


e At both design and control levels, the mechanical engineer has a modelling role which 
consists of building various realistic models of mechanical behavior of the robot. These 
will be used mainly for two purposes: for off-line simulation of the system with given degree 
of fidelity and for its control using adequate control modes. The mechanical engineer will 
also have to define the convenient analytical tools to plan and describe mathematically 
the trajectories of the system. 


1.7  Multidisciplinary aspects of robotics 


Let us note that the mechanical structure of a robot is only the "visible part of the iceberg’. 
Robotics is an essentially multidisciplinary field in which engineers from various horizons such 
as electrical engineering, electronics and computer science play equally important roles. 


'Therefore it is fundamental for the mechanical engineer who specializes in robotics to learn to 
dialogue with them by getting a sufficient level of understanding in these disciplines. 
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2 CHAPTER 2. BASICS OF ROBOTICS TECHNOLOGY 


Figure 2.1.1: Position and orientation of a rigid body specified through the position of three 
non collinear points 


2.1 The Mechanical Structure of a Robot 


'The mechanical structure of a robot manipulator may be regarded as a multibody system, i.e. 
a system of bodies or links, rigid in theory, interconnected together by joints or kinematic pairs. 


2.1.1 Degrees of freedom of a rigid body 


Let us consider a rigid body freely located in space. It is possible to specify both its position 
and orientation in space by giving the position of three non collinear points attached to it. 


'This is a set of 9 non independent parameters, since the coordinates of these points are linked 
through three relationships expressing that the distance between them is fixed. 


In the general case, the geometric location of a free rigid body is described with 6 parameters 
or degrees of freedom (DOF) which may be classified in two categories: 


e 3 independent translation parameters which define the location in space of a reference 
point on the solid; 


e 3 independent rotation parameters which define the orientation in space of the solid. 


It will be seen in the chapter devoted to the kinematics of the rigid body that the three trans- 
lation DOF may be expressed indifferently in terms of any system of coordinates (cartesian, 
cylindrical, polar, etc.) while the orientation of the body is specified through a set of rotation 
parameters such as Bryant angles, Euler angles or parameters, etc. (as it will be seen later). 


2.1.2 Joints and kinematic constraints 


Joints or kinematic pairs connect the different bodies or links of the multibody system to one 
another. They impose constraints between the motions of the connected bodies. 

Basically, each joint can be replaced by a set a algebro-differential constraints. Thus the 
effect of m constraints restraining the motion of a system of N bodies is the following: Instead 
of having 6N variables describing the system behaviour, the multibody system has only 6N — m 
degrees of freedom left. 
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2.1.3 Generalized coordinates 


The choice of a minimal set of independent variables, which are able to describe fully the multi- 
body system is of a primary importance in mechanics. This minimal set of variables is named 
as the set of generalized coordinates of the system. The choice has a great impact on how 
easily one will be able to solve the motion’s equations afterwards. For example an advantages of 
generalized coordinates is that constraints may often be automatically (implicitly) be satisfied 
by a suitable choice. This eliminates the needs for writing out separately 6N equations of motion 
subject to m equations of constraints. 


2.2 Kinematic Pairs 


2.2.1 Number of degrees of freedom of the joint 


One major characteristic of a joint is the number of geometric constraints it imposes between the 
two connected bodies. The number of degrees of freedom DOF is equal to the minimal number 
of parameters necessary to describe the position of C2 relatively to Cı. A kinematic pair is 
thus characterized by 5, 4, 3, 2, or 1 DOF and its class is defined as the complement to 6 of its 
number of DOF. Clearly, the class of the joint is the number of geometric constraints imposed 
by the joint. 


2.2.2 Classification of joints 


Kinematic pairs between two rigid bodies C4 and C2 can be also formally classified according 
to various criteria such as: 


e Holonomic and non holonomic constraints 


'The kinematic pair may introduce a constraint, which depends only on the positions of 
the two bodies and not on their velocities. This results in purely algebraic constraints. 
'These constraints are said to holonomic constraints. 


Otherwise when pair introduce contraints that are dependent of the velocities too, they 
are said to be non holonomic and the contraints result in a algebro-differential relations. 
A disk rolling without slipping on a surface is a non-holonomic constraint, while the fixed 
distance between two points of a rigid body is a holonomic one. 


e The type of relative motion allowed 


The motion of any point on C» relatively to C1 can be restrained to a line, or surface or 
be arbitrary in space; 


e The type of contact 


'The contact can be point -, linear - or surface contact; 


e The mode of closure 


A kinematic pair is self-closed if the contact between bodies is guaranteed by the way 
the kinematic pair is realized. Otherwise, it can be force-closed, which means that an 
external force (such as provided either by gravity or through the compression of a spring) 
is necessary to maintain contact. 
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Figure 2.2.1: The three possible lower pairs (a) revolute joint (R); (b) prismatic joint (P); (c) 
screw joint (H) characterized by only one DOF and reversible motion 


Figure 2.2.2: The three possible lower pairs characterized by more than one DOF and reversible 
motion : (a) cylindrical joint (C) with 2 DOF; (b) spherical joint (S) with 3 DOF; (c) planar 
joint (E) with 3 DOF 
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2.2.39 Higher and lower pairs 


Following Reulaux, one can distinguish higher and lower kinematic pairs. 


e In lower pairs the elements touch one another over a substantial region of a surface. There 
are only six lower pairs, namely the revolute joint (R), the prismatic joint (P), the screw 
joint or helical joint (H), the cylindrical joint (C), the planar joint (E), and the spherical 
or globular joint (S for spherical or G for globular). The six lower pairs are represented 
in figures 2.2.1 and 2.2.2. Figure 2.2.1 gives the three class 5 lower pairs. All three of 
them have only one DOF. In the form represented, the pairs are self-closed. Figure 2.2.2 
illustrates the three other lower pairs that are of class 4 and class 3. In the implementation 
represented, the planar joint is not self-closed. 


Even if contact area has been used as the criterion for lower pairs by Reuleaux, the real 
concept lies in the particular kind of relative motion permitted between the two links. 
In lower kinematic pair, coincident points on Cı and C2 undergo relative motions that 
are similar. In other words, the relative motion produced by a lower pair can be said 
reversible, and an exchange of element from one link does not alter the relative motion 
of the parts. That is, the relative motion between links C1 and C3 will be the same no 
matter whether link C1 or link C5 is the moving link. 


e In higher pairs the elements touch one another with point or line contact. The relative 
motion of the elements of of higher pairs is relatively complicated. An infinite number of 
higher pairs exist, and they may usually be replaced by a combination of lower pairs. 


Examples of point contact can be found in ball bearings and helical gears on non-parallel 
shafts. Also the Hooke or universal joint (U or T) can be seen as having an idealized 
unique contact point. Line contact is characteristic of cams, roller bearings and most 
gears. The rigid wheel is also and example of line contact. 


One must also notice that the two elements may touch as wheel at two non material body 
fixed surfaces, named the circle-point curve and the center-point curve along the actual 
axis of rotation. 


2.2.4 Graphic representation of joints 


Several norms propose a conventional graphic representation of kinematic pairs. As an example, 
the French norm AFNOR is given in figure 2.2.5. To the six pairs that we have just described, 
have been added the limiting cases of the “rigid pair" (0 DOF) and the “free pair” (6 DOF), 
as well as three other higher pairs: the linear contact joint, the annular contact joint and the 
point contact joint. 


2.2.5 Joints used in robots 


Let us note that most of these joints are not used in robot design, at least as far as the relative 
motion of links is concerned. In order to analyze the external architecture of robot manipulators, 
it is sufficient to consider 


- the revolute joint (R), which produces a relative rotary motion; 


- the prismatic joint (P), which produces a relative translation motion. 
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bevel-wheel-gear (spherical) worm-gear (spherical) 


Figure 2.2.3: Examples of higher pairs 
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Figure 2.2.4: Building up joints from revolute and prismatic joints 
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Figure 2.2.5: AFNOR E04-E015 representation of kinematic pairs 
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Figure 2.2.6: Schematic representation of revolute and prismatic pairs 


Both pairs are commonly represented as shown on figure 2.2.6. 


Other kinematic pairs may also appear when examining the mechanical structure of the the 
transmission components. For example, the screw joint has the interesting property to transform 
rotary motion into translation motion, and conversely; it allows thus to power a prismatic joint 
using an actuator with rotary output. 


2.3 Topology of Kinematic Chains 


Starting from a reference body Co forming the basis of the manipulator, the bodies C; and the 
kinematic pairs L; forming the mechanical structure of the robot can be attached together in a 
wide variety of manners. The topology or morphology of the kinematic chain is the study of the 
relative position of the different links and joints of the chain, and the way they are connected 
to each others. This characteristic has a major impact on its performance of the system . 


2.3.1 Classification of robot topologies 
The morphology of the manipulator can be classified into differents groups: 


e Simple open-tree structures (figure 2.3.1) 


'The simple open-tree structure is characterized by the fact that all the links and joints 
are connected in a row, that is every body C; in the kinematic chains is connected only 
to the two neighboring elements C;.; and C;,1. Most of the industrial robots currently 
commercialized possess such a purely serial architecture. 


e Multiple open-tree structures (figure 2.3.2) 


'The multiple open-tree structure is rarely used in robotics. It offers the possibility to 
integrate in a single system several effectors, and is sometimes used for that reason. Let 
us note that such a morphology corresponds to that of the human body. 


'The connectivity in this kind of architecture is such that the bodies C; can no longer be 
numbered in sequence since some of the links are implied in more than two kinematic 
pairs. To describe it, it is necessary to use the concept of graph. 


Remark that in kinematical chains with open-tree structure (simple or multiple tree struc- 
tures), there is only one path from each body to another one. Therefore it is possible to 
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Figure 2.3.2: Multiple open-tree structure 
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Figure 2.3.3: Multiply-connected structure 


relate each body or joint to a following one clearly. Furthermore, if ng is the number of 
bodies and if n; is the number of joints, one shows that there are equal: 


nB = NJ (2.3.1) 


Multiply-connected or complex structures (figure 2.3.3) 


The multiply connected (or complex) structure is characterized by the appearance of 
mechanical loops, i.e. paths which start from and come back to a same body after having 
followed certain sequences of links and joints. 


A significant number of industrial robots currently on the market have such a kinematic 
architecture. The use of mechanical loops allows one to build a structure with higher 
structural stiffness, and thus obtaining a greater working accuracy. However, multiply- 
connected structures have also generally a lower mobility. The importance of complex 
structure is growing nowadays very fast with the study and the introduction of parallel 
manipulators to new applications such as High Speed Machining and metrology of large 
size pieces. 


Let us also note that most manipulators exhibit an external architecture of simple open- 
loop type, but have to be regarded as multiply-connected structures as soon as one takes 
into account the model of the transmission devices. 


One can see that a kinematical chain with tree structure can be changed into an indepen- 
dent closed loop by introducing an additional joint. So if nz is the number of closed-loops, 
one can see that: 

np = NJ— NB (2.3.2) 


The kinematic analysis of multiply-connected systems implies being able to model struc- 
tures of multiple open-tree type: indeed, any multiply-connected system may be reduced 
to a multiple open-tree linkage by removing a number of pairs equal to the number of 
mechanical loops. The closure conditions are then treated as separate constraints. 


As underlined earlier, the morphology of the manipulator has a major impact on its charac- 
teristics. For instance, figure 2.3.4 displays three relatively common architectures of industrial 
robots. The first one is a simple open-tree structure in which all the kinematic pairs are of rev- 
olute type. It generates a workspace having the shape of a portion of sphere. The second one is 
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Figure 2.3.4: Common architectures of robot manipulators (a) simple open-tree structure with 
revolute joints (RRR) (b) simple open-tree structure with prismatic joints (PPP) (c) multiply- 
connected structure 


also a simple open-tree structure, but has all prismatic joints. The workspace generated is thus 
a parallelipiped. The third one has a multiply-connected architecture with all revolute joints. It 
has a closed loop in the form of a parallelogram which gives to it the properties of a pantograph. 
Its 3-dimensional motion can be produced by rotating the waist and translating into vertical 
and radial directions one of the vertices of the pantograph. The resulting workspace has thus a 
cylindrical shape. It comes that the choice of a proper morphology of the manipulator must be 
chosen carefully when selecting a robot or designing it. 


2.3.2 Description of simple open-tree structures 


From what preceeds, it can be observed that any robot having a simple open-tree structure may 
be described by a sequence of n letters (n being the number of joints) R and P which define the 
sequence and the type of kinematic pairs crossed when progressing from the reference body to 
the effector. For example, the simple open-tree structures represented by figures 2.3.4(a) and 
2.3.4(b) are respectively of types RRR and PPP. 


2.4 Mobility Index and Number of dof for a Simple Open- 
Tree Manipulator 


2.4.1 Mobility index and GRUBLER formula 


Except for robots with a mobile base, the reference body Co is fixed in space, while the other 
members C, to Cn are mobile. Before assembling the system, the position and orientation of 
the n links are completely arbitrary in space, and are thus defined in terms of 6n parameters. 


Let us consider a kinematical chain with: 
e np the number of bodies (without the base body, which is assumed to be fixed), 
e nz the number of joints, 


e f; the number of degrees of freedom of joint j, i.e. joint j is of class c; = 6 — fj. 
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'The total number of degrees of freedom, i.e. the necessary number of free parameters to deter- 
mine completely its geometric configuration of the system is given by the formula: 


nj 
M = 6ng — M(6- fj) 
j=l 
or 
nj 
M = 6(ng-nj) + M fj (2.4.1) 
j=l 
With the total number of kinematical loops 
np = nj NB 
the total number of degrees of freedom is: 
nj 
M = Mf; - 6nr (2.4.2) 
j=l 


Equations (2.4.1) or (2.4.2) are known as Griibler’s formula. 
Based on Griibler’s formula, one finds the Griibler’s criterion of mechanisms. The following 
cases are distinguished: 


e M =1, mobility-1 mechanism; 
e M =Q, statically determined structure; 


e M <0, statically undetermined structure. 


Important remarks 


In some cases formula (2.4.1) or (2.4.2) lead to wrong results. A first situation is the case of 
planar or spherical kinematical chains, in which each body is constrained to move in a plane 
or along the surface of a sphere. Then, each body has only 3 degrees of motion. Thus formula 
formula (2.4.1) and (2.4.2) become: 


M = 3(ng-nj) + Sof; (2.4.3) 
j=l 
M = VM - 8n; (2.4.4) 
j=l 


Formula (2.4.1) or (2.4.2) may also be erroneous when they are applied to complex architectures 
such as that of figure 2.3.4(c) since the formula assumes that all the constraints imposed by 
the joints are independent of one another. However sometimes, mechanical loops introduce 
redundancies between the geometric constraints of the different joints and thus, they do not 
necessarily reduce the effective mobility of the mechanism. 
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2.4.2 Number of dof of a simple open-tree structure 


If we limit ourselves to a simple open-tree structures made of kinematic pairs of types R and P 
(class 5) for which f; — 1 for all j, then the formula above reduces further to 


M ERES UM (2.4.5) 


This formula means that the mobility index of the manipulator is then equal to its number 
of joints. Therefore, since 6 parameters are necessary to describe an arbitrary position and 
orientation in space for the effector, a simple open-tree robot manipulator possesses normally 6 
joints of either revolute or prismatic type. 


2.4.3 Number of DOF of a manipulator 


The number of DOF N of a robot manipulator is equal to the number of independent parameters 
which fix the location of the effector. It can be a function of the geometric configuration of the 
robot, but the following inequality is always verified 


N<M (2.4.6) 


2.4.4 Joint space 


To represent the configuration of the manipulator and the position of all its bodies, the most 
obvious solution is to use the the joint variables or joint coordinates, which are the degrees of 
freedom of the kinematical chain. Therefore they are sometimes known as configuration variables 
of the robot. In real robots, these degrees of freedom are motorized. 

To each set of joint variables, one can attach a point in a hyper-space R™ of dimension M 
with M the number of joint variables (equal to the mobility index of the kinematical structure). 
The hyper-space R™ that is used to represent the configuration variable sets is the joint space 
of the robot. 


2.4.5 Task space 


The configuration of the effector, i.e. the tool of the robot, is usually given in terms of the 
cartesian coordinates of one reference point (called the Tool Center Point or TCP) and the 
orientation of the tool about this reference point. For 3-dimensional problems, the coordinates 
of the Tool Center Point can be associated to a point in IR?, but for the orientation things are 
more complicated: the 3 independent rotation variables can be related to the group SO(3) of 
all rotations in IR?. Thus the tool configurations belongs to the space R? x SO(3). This space 
of all tool configuration point is the task space. As SO(3) is not a vector space, the task space 
is not a vector space either. If the tool has got other additional degrees of freedom related, for 
instance the configuration state - open or closed - of the gripper, they must also be added to 
the configuration space. 


2.4.6 Redundancy 


The case ng < M corresponds to the existence of a redundancy in the DOF of the system. In 
other words, a robot is redundant when the number of degrees of freedom of its effector is less 
than its index of mobility, i.e. the number of generalized degrees of freedom. 
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Figure 2.4.1: Illustration of a redundant manipulator 


This situation can be viewed as loss of mobility coming from a bad design of the manipulator. 
However, this kind of architecture may sometimes be desired because, redundancy allows to 
increase the volume of the design space while preserving the capability of moving the effector 
around obstacles. Indeed the extra degrees of freedom generally allow to circumvent the obstacles 
in the environment. The Figure 2.4.1 illustrates the case of a redundant manipulator, which 
allow to avoid obstacles in the workspace. 


The following joint combinations in simple open-tree structures give rise to redundant struc- 
tures: 


e More than 6 joints; 
e More than 3 revolute joints with concurrent axes; 


e More than 3 revolute joints with parallel axes; 


More than 3 prismatic joints; 
e Two prismatic joints with parallel axes; 


e Two revolute joints with identical joint axes. 


When the robots has several effectors (multiple open-tree structures) the redundancy is 
evaluated in comparing the number of degrees of freedom acting on each tool and the number 
of degrees of freedom of the effector under consideration. 


Let us consider first the example of Figure 2.4.2 which shows two distinct robot architectures, 
both of simple open-tree type, made of two links connected by two prismatic joints. According 
to the definition both have a mobility index M = 2. In case (a), the effector remains in a plane 
and keeps a constant orientation, hence ng = 2. In case (b), however, both kinematic pairs 
give freedom in the same direction, so that the effector is constrained to move along a constant 
line. We have in this case ng = 1: the structure represented in case (b) has thus a redundant 
kinematic architecture. 


2.4.7 Singularity 


Let us consider next the example of figure 2.4.3 which shows the same manipulator made of 
three links, with two prismatic and one revolute joint. The mobility index is now M = 3. 
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(a) (b) 
M=n,= 2 M=2,n,=1 


Figure 2.4.2: Comparison of the mobility index and the number of DOF for two robot manipu- 
lators with two prismatic joints 


(a) (b) 


Figure 2.4.3: Comparison of the mobility index and the number of DOF for two robot manipu- 
lators with two prismatic joints 
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Figure 2.4.4: Planar and spherical four-bar mechanisms 


The effector is constrained to move in a plane with arbitrary orientation, hence ng = 3. However, 
in the particular configuration shown in (b), both prismatic joints give mobility in the same 
direction: therefore, ng — 2. Such a configuration which gives rise to a local redundancy is 
called singular. 


Generally speaking, for any robots, redundant or not, it is possible to discover some configu- 
rations, called singular configurations, in which the number of freedoms of the effector is inferior 
to the dimension, in which it generally operates. 


Singular configurations happens when: 
e Two axes of prismatic joints become parallel; 


e Two axes of revolute joints become identical. 
Remarks 


It will be seen later some analytical methods to determine the number of degrees of freedoms of 
the effector and singular configurations. 


A robot that is not redundant in general can be redundant when considering a given task. 


2.4.8 Examples 
Planar and spherical four-bar mechanisms 


Let's consider the planar and spherical mechanisms of figure 2.4.4. On identifies: 
Number of bodies: np — 3 
Number of joints: n; = 4 with a number of degrees of freedom of each joints fj = 1 
Number of closed loops nr, = 1 

Using formula (2.4.3) and (2.4.4), one finds: 
Number of degrees of freedom M — 1 (Mobility 1 mechanisms) 


7-revolute-joint mechanism 


A general spatial mechanism is presented in figure 2.4.5. Its characteristics are the following: 
Number of bodies: np — 6 
Number of joints: n; = 7 with a number of degrees of freedom of each joints fj = 1 
Number of closed loops nz = 1 

Introducing these data in formula (2.4.2) gives 
Number of degrees of freedom M — 1 (Mobility 1 mechanisms) 
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Figure 2.4.6: Spatial four-bar mechanism 


Spatial four-bar mechanism (special 7-revolute-joint mechanism) 


Let's consider the general spatial mechanism of figure 2.4.6, which is a particular case of the 
general spatial mechanism of figure 2.4.5. Indeed, universal joint can be replaced by 2 revolute 
joints, while the spherical joint is equivalent to 3 concurrent revolute joints. According to the 
previous analysis, this mechanism is a 1 degree of freedom mechanism. This kind of device is 
used to transmit a rotation motion 3, towards a output rotation (2. 

If the coupler is connected to the two links by two spherical joints, there would be one 
additional degrees of freedom, the rotation of the coupler about its longitudinal axis. This 
"isolated" degree of freedom does not influence the transmission behaviour, i.e. the relation 


b2 = f(1). 
SAR antenna 


The SAR antenna mechanism presented in figure 2.4.7 is a planar mechanism. One particular 
characteristic of the mechanism is the presence of multiple joints. Each multiple joint can be 
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Figure 2.4.7: SAR antenna 


broken into several individual revolute joints that connect a reference body and the other ones. 
Number of bodies: ng = 8 
Number of joints: n; — 11 with a number of degrees of freedom of the joints f; — 1 
Number of closed loops nr, = 3 
Number of degrees of freedom M = 2 


Five-point wheel suspension 


'To determine the total number of degrees of freedom, it is necessary to take into account the 
isolated rotation of the rods around their longitudinal axis. Therefore the joints at the car body 
are modeled as universal joints. 
Number of bodies: ng = 7 
Number of joints: n; — 11 
A number of degrees of freedom of the joints fj = 1 for revolute joints (R) 
f; = 2 for universal joints (T) 
f; = 3 for spherical joints (S) 
Number of closed loops nr, — 4 
Number of degrees of freedom M = 2 
The two dof of the suspension are the spring deformation and the steering gear angle. 


2.4.9 Exercices 
Singular and redundant manipulators 


For the four robots sketched in figure 2.4.9, identify the redundant manipulators and the singular 
configurations. 
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Figure 2.4.8: Five-point wheel suspension 
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Figure 2.4.9: Illustration of redundant and singular manipulators 
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Figure 2.4.10: General Electric P5 robot 


Number of dof of mechanisms and manipulators 


For the robots given in figures 2.4.10, 2.4.11, and 2.4.12, find the number of degrees of freedom. 
Do the same for the wheel suspension given in 2.4.13. 


2.5 Number of dof of the task 


The number of dof nr of a given task is equal to the number of independent parameters necessary 
to describe the position and orientation of the effector for all the configurations that it has to 
reach during the task. 

Generally speaking, to give an arbitrary location to the effector (position and orientation) 
requires 6 dof. However, because of the geometry of the tools or of the pieces on which the work 
is made, many tasks may be realized with a lower number of degrees of freedom, i.e. many tasks 
are characterized by a number of DOF nr < 6. For example: 


e Any manipulation task involving a cylindrical piece is such that nr < 5, since the task is 
not influenced by a rotation about the revolution axis (see Fig2.5.1, a/) ; 


e Any task of vertical insertion (such as insertion of components in electronic assembly) 
implies at most 4 parameters: positioning and orientation in a plane, vertical positioning 
(see Fig2.5.1, b/). Hence nr < 4; 


e Positioning a spherical object requires nr = 3 degrees of freedom, because the task is not 


influenced by any rotation (see Fig2.5.1, c/). 


In other words, a necessary but non sufficient condition to insure the compatibility of the 
manipulator and the specified task to perform is that: 


nR = np (2.5.1) 


Under these conditions, there is an adequacy of the manipulator for a specified task, i.e. the 
possibility of finding a configuration of the robot manipulator which makes possible to reach 
each specified location of the effector depends upon it. 
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Figure 2.4.13: Wheel suspension 
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a/ 3 dof positioning task — b/ 4 dof insertion task c/ 5 dof drilling task 


Figure 2.5.1: Number of dof of different tasks 


Table 2.1: Number of morphologies as a function of the number of dof of the simple open-tree 


manipulator 
Number of dof of the robot | Numbers of possible structures 


2.6 Robot morphology 


In the later chapter, our study will focus on the open-tree structure robots. Therefore, one is 
considering here mainly the different open-tree morphologies. 


2.6.1] Number of possible morphologies 


In order to consider the different architectures, one identifies two parameters: 
e The type of joint (revolute -R- or prismatic -P- joint); 


e The angle between two successive joints: 0 or 90?. Indeed, except in some very particular 
cases, the successive joints are either parallel or perpendicular. 


The number of different configurations (see 2.1) comes when combining those parameters. 


2.6.2 General structure of a manipulator 


The general architecture of a robot may be divided into three parts (see Figure 2.6.1): the base 
or carriage when it is mobile, the arm, and the wrist. For fixed robots, the arm of the robot 
is usually the 3 first degrees of freedom (either of revolute or prismatic type), while the other 
remaining degrees of freedom, which are characterized by lower sizes and a lower mass are the 
the wrist. 

Based on Table 2.1, one can enumerate 36 morphologies for the arm. Among these 36 basic 
configurations, only 12 are mathematically different and non redundant (one does not consider 
the configurations of the arms that gives rise only to linear or planar motions, for instance 3 
parallel prismatic joints or 3 parallel revolute joints. 
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Figure 2.6.1: Uncoupling between effector position and orientation 


The mechanical role of the arm is to position the effector in space. The wrist is in a charge 
of giving the prescribed orientation to the tool. 


2.6.3 Possible arm architectures 


The most usual architectures of the arm (with a simple open-tree mechanical structure) derive 
from the various systems used to define space coordinates. The four most common architec- 
tures are represented by figure 2.6.2. They correspond to the kinematic pair sequences PPP, 
RPP, RRP and RRR. Some of them have been used historically, others are current industrial 
solutions. 

More recently, a fifth possible architecture has been introduced (figure 2.6.3): RPR, PRR or 
RRP, which is based on the principle of translating along the prismatic hinge a articulated 
coplanar mechanism. The resultant structure is called SCARA robot (Selective Compliance 
Assembly Robot Arm), which is very efficient for planar assembly operations such as insertion 
of mechanical and electronic components. This type of robot it is now gaining an increasing 
part of the market. 

According to a study [7] realized by the Association Francaise de de Robotique Industrielle 
(AFRI) and the Journal RobAuto in 1997, there is a large majority of RRR type robot (44%), 
then follow the cartesian manipulators (20,596), cylindrical arms (7%), and SCARA robots 
(7%). 


2.6.4 Kinematic decoupling between effector orientation and position 


Most of the robots have a wrist made of three revolute joints with intersecting axes and orthog- 
onal two by two. Even if there are other types of wrist architecture, the wrist of spherical type 
is very important. Indeed this architecture leads to the following fundamental properties: 


e The point O, called the center, plays the role of a virtual spherical joint; 


e The position position of this virtual spherical joint depends on nothing else than the 
displacements at hinges Lı, Lə and L3; 


e The displacements at joints L4, L5 and Lg determine the orientation of the effector about 
point O. 
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Figure 2.6.2: Most common robot architectures (a) cartesian coordinates: PPP architecture 
(b) cylindrical coordinates: RPP architecture (c) polar coordinates: RRP architecture (d) 
universal coordinates: RRR architecture 


Figure 2.6.3: PRR or SCARA robot architecture 
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It is easy to perceive the practical consequences of such a decoupling: It allows to break the 
problem of determining the six configuration parameters into two independent problems, each 
one with only three parameters. 


2.7 Workspace of a robot manipulator 


2.7.1 Definition 


Intuitively the position workspace of a robot manipulator is the physical space generated by a 
specified point on the tool when the system is taking all possible geometric configurations. Let 


us note the relative inaccuracy of this definition, for the following reasons: 


e The choice of a point on the tool is arbitrary. Some manufacturers take as a reference the 
center O of the wrist if it exists, and obtain in this manner the position workspace with 
least volume. Others take as a reference either a point located at the end of the tool, or 
a point located at its fixture; 


e The possible orientations of the tool at a given point of the workspace are not taken into 
account in this definition. Some authors complement it by introducing the concept of 
dexterity. The dexterous workspace of a manipulator is then defined as the subspace of the 
position workspace in which the wrist can generate an arbitrary orientation of the tool. 


More precisely, one distinguishes [3] the following different workspaces : 


e Workspace of a robot : The set of all positions and orientations that can be reached by a 
given frame attached at the effector of the robot while the joint degrees of freedoms are 
allowed to take all their admissible values. 


e Reachable workspace: The set of all positions that a given point of the effector of the robot 
can reach for at least one orientation while the joint degrees of freedoms are allowed to 
take all their admissible values. 


e Dexterous workspace or primary workspace: The set of all positions of the workspace that 
can be reached with all possible orientations. Secondary workspace: The set of all positions 
of the workspace that can be reached with some specified orientations. Workspace with a 
given orientation 


Of if the workspace is the set of points in IR? x SO(3), which is impossible to draw. It is 
fairly common to represent the workspace with two orthogonal sections chosen according to 
the type of manipulator considered. This geometric representation is generally preferable to a 
perspective view. 


2.7.2 Comparison of the workspaces with different arm configurations 


In order to compare the different architectures described in section 2.7, it is necessary to adopt 
the following assumptions: 


e The "length" of each member is equal to L; 


e The rotation allowed to each revolute joint is 360 deg; 
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Figure 2.7.2: Structure RPP: toric workspace with square section internal radius L, external 
radius 2L, volume V = 3a L? c 91? 


e The translation allowed to each prismatic joint is equal to L. 


Under these conditions, the position workspace corresponding to each one of the five basic ar- 
chitectures described in section 2.7 is represented hereafter (exceptionally, in perspective form). 
'These volumes correspond to a manipulator having an arbitrary wrist with its center located at 


O. 


This comparison demonstrates the higher mobility of the architectures RRP and RRR, since 
they possess a workspace volume about 30 times larger than the PPP architecture. The RPP 
and RPR, on the other hand, provide a volume of intermediate magnitude. 


The present analysis of workspace does not provide any information on the dexterity properties 
of these structures such as the ability to go around obstacles or to penetrate a cavity. 


2.7.3 Workspace optimisation 


An effective evaluation of the workspace has to take into account the displacement range allowed 
to revolute and prismatic joints as well as the length ratios of the members. The architectures 


2.7. WORKSPACE OF A ROBOT MANIPULATOR 27 


Figure 2.7.3: Structure RRP: hollow spherical workspace internal radius L, external radius 2L, 
volume V = 28/31 L? ~ 29L? 


Figure 2.7.4: Structure RPR: cylindrical workspace radius 2L, height L, volume V = AnaL? ~ 
13L? 


Figure 2.7.5: Structure RRR: spherical workspace radius 2L, volume V = 32/31 L? ~ 34L? 
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Figure 2.7.6: Planar manipulator with 2 DOF 


of types PPP, RPP and RRP remain easy to analyze and it is relatively straightforward to 
maximize the volume of their workspace for a prescribed elongation. 


The problem becomes more complex for PRR and RRR structures and the solution is no longer 
obvious. 


Figures 2.7.7 show different workspace configurations for a planar manipulator with 2 DOF. 


In both latter cases, the problem can be reduced in a first step to a planar one: the 3-dimensional 
workspace is then obtained by sweeping through either translation or rotation the geometric 
figure 2.7.7, obtained in the plane. 


Let us thus consider the planar mechanism of figure 2.7.6 such that 
e The maximum elongation L = £4 + 2 is specified; 
e The move limits are given on both DOF 6; and 65: 
bim < 01 € Aim and 025, € 05 < Oem (2.7.1) 


According to the variation domain of 05, the workspace can take one of the fundamental 
shapes represented on figure 2.7.7 


Evaluation of the swept area 


'The area of the domain swept by point O is evaluated by 


A= fa dy = n J| dO, 02 (2.7.2) 
where J is the Jacobian matrix of the coordinate transformation 


= ,cos6, + f5cos(04 + 02) 
f1sin04 + £5 sin(61 + 02) (2.7.3) 


—4 sin 01 = bo sin(01 + 62) —(» sin(01 + 05) 


HU £1cos01 + /5cos(01-- 05) | (5cos(0; + 02) 


(2.7.4) 


2.7. 


WORKSPACE OF A ROBOT MANIPULATOR 


Workspace of first type 


0 € 05,, € 02 € bay < T — T € dam € 05 < day <0 


Workspace of second type 


0 € 024, € 02 < 00m, Ou mm Bam € —7, Om < 05 < bay <0 


Workspace of third type 


055, < 02 X024, =T «054 «0, O< 2m <T 


Figure 2.7.7: Possible shapes of the workspace for a planar manipulator with 2 DOF 
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from which one deduces that 
det J — libo sin 05 (2.7.5) 


Therefore the area swept by the point O is given by 


02M 
A= Ly bo (014 -ew f | sin 65|d05 (2.7.6) 
025. 
or, formally 
A = biba (1m — bim) (82m 02m) (2.7.7) 


Optimization of the swept area 


Optimization of the length ratio: the maximal elongation L = £4 + £9 being specified, the ratio 


A= - will maximize the volume 


A= aix] (01m — fim) f (025, 024) (2.7.8) 
if 
a =0 and T «0 (2.7.9) 
It is observed effectively that 
a = dias] (fim — 01.) f (025,024) = 0 for A= (2.7.10) 


and 


dA 2(A—2) 3 
dX = doxes (01M — him) f (0255, 92m) < 0 for A=1 (2.7.11) 


We may thus conclude that the workspace area is the largest when both links have equal lengths. 


Optimisation of 05,, and 02,4 


The integrand of equation (2.7.6) 
£165 |sin 02| (0114 — 014.) (2.7.12) 


represents the variation rate of the workspace with respect to 02. It vanishes if 02 = 0 and reach 
its maximum when 62 = +7/2. It means that a variation of 02 centered about +7/2 allows 
to sweep a maximal area. This conclusion is confirmed by the table of figure 2.8.11 for the 
particular case where ım — 014, = 90 deg and £4 = £9 = 1. 


This simplified analysis leads to the conclusion that a kinematic architecture of types RRR 
or PRR provides a good design if the links have equal lengths and that the variation range for 
05 is centered around +90 deg. It is interesting to observe that the human arm matches these 
conditions. 
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Figure 2.7.8: Variation of workspace area with angular range 05 
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Figure 2.8.1: Repeatability and accuracy experiment of a robot 
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2.8 Accuracy, repeatability and resolution 


In order to explain these three complementary concepts, let us consider the following experiment 
(see fig 2.8.1). An arbitrary robot at rest in a configuration A and programmed in such a way 
that its effector is moved and stops in a specified configuration B. The experiment is repeated 
a large number of times and statistical processing can be realized of the results. 


Let us repeat this operation a large number of times: there exists an average location Bm 
corresponding to the mean value of the locations reached during the successive displacements. 


2.8.1 Static accuracy 


The deviation between the programmed situation B and the average location Bm provides a 
measure of the static accuracy of the manipulator. It characterizes the ability of the manipulator 
to locate the tool in accordance to the programmed situation: it can be a function of the 
geometric configurations A and B themselves and of the trajectory selected to perform the 
motion. It may also depend on parameters such as 


e the static accuracy of the feedback systems controlling the actuators; 
e the structural flexibility of members and joints; 


e the load carried by the tool; 


the friction in backlash in joints; 


the resolution of the position and velocity sensors; 


e ctc. 


When account is taken of all the possible sources of error, it is found that the static accuracy 
of most industrial robots, when measured at tool level, does not exceed the following values: 


e Positioning accuracy: a few millimeters; 


e Orientation accuracy: a few tenths of degrees. 


2.8.2 Repeatability 


The deviation between the average location B,, and the successive locations reached effectively 
gives a measure of the repeatability of the manipulator. Since from its very definition the 
repeatability is not affected by any systematic cause of inaccuracy, it is generally much better 
than the static accuracy. 


2.8.8 Resolution 


The resolution of a robot manipulator is the smallest distance between the initial configuration 
A and the programmed configuration B which generates an effective change in the location of 
the tool. It may depend on 


e the resolution of the position and velocity sensors; 
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e the word length used in arithmetic computations; 
e the gear ratio of the transmission chains. 


It is important to have clearly in mind the fundamental difference between these three concepts. 
Let us note that most manufacturers, in the technical notices, provide only a measure of the 
repeatability. 


2.8.4 Normalisation 


The problem of repeatability and static accuracy of robots is a primary importance for industrial 
robot applications. This led to a normalization work from ISO and it has been written down in 
the normalisation document ISO 9283, which describes the tests conditions under which these 
characteristics can be measured. This document specifies also the other performances of robots 
in order to insure their comparison for the choice. Additionally, normalization allows also the 
users to assess the interchangeability of robots in production conditions. 


2.9 Robot actuators 


We have up to now described the arm and wrist architectures which form together the mechanical 
structure of the robot. Let us now have a quick look to the way this mechanical structure is put 
into motion. For activating each joint, motorization implies 


e To provide a primary energy, most often in pneumatic, hydraulic or electrical form; 

e To modulate the energy brought to the system; 

e To convert the primary energy into mechanical energy; 

e To transmit the mechanical energy to the joint; 

e To control and measure the motion variables (position, velocity, acceleration, force, etc.) 


Figure 2.9.1 summarizes these different functions. Production and modulation of energy are 
functions which fall completely out of our scope. We will briefly describe the functions of energy 
conversion by the actuators, transmission of the mechanical energy by the transmission chains 
and position and velocity measurement by the sensors. 


The selection of appropriate transmission chains greatly differs according to the choice of either 
distributed motorization or centralized motorization: it is thus important to have some ideas 
about the advantages and drawbacks of both solutions. 


2.9.1 Distributed motorization 


The use of actuators which drive directly the joint axis on which they act looks like the simplest 
solution that can be adopted for obtaining an efficient design. Presently this solution is however 
not the most common one, for a certain number of technological reasons: 


1. Most of the actuators provide a high velocity output with low torque, while what is needed 
is to provide a high torque at low velocity. A tramsmission system is thus needed to adapt 
the mechanical impedances of the actuator and its load: even when distributed motricity 
is adopted, in many cases the use of a speed reductor cannot be avoided. 
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Figure 2.9.1: Motorization of robot joints 


2. The actuator equipped with the appropriate speed reductor has a non negligeable volume 
and locating it at the joint might reduce the mobility of the manipulator in a significant 
manner; 


3. Likewise, each actuator equipped with its speed reductor possesses a mass and a rotary 
inertia which have to be added to that of the passive structure; this additional load has to 
be into taken account in the sizing of the system and may significantly affect its dynamic 
behavior. 


2.9.2 Centralized motorization 


In opposition to the previous solution, it is possible to locate in a systematic manner all the 
actuators at the basis of the manipulator and transmit the motion to the joints through appro- 
priate transmission chains. Obviously, the transmission components have also their own weight 
and generate a significant weight penalty for the mechanical structure. Moreover, centralized 
motorization raises some specific problems such as 


1. Transmitting the motion to several joints through a mechanical structure with varying 
geometric configuration can only be achieved at the cost of a complex internal kinematics; 


2. The friction between mechanical components generates significant losses of energy and 
reduces the accuracy of the system (backlash, hysteresis); 


3. The low stiffness of some mechanical components may be the origin of vibration and jerk 
in the structure. 


2.10. MECHANICAL CHARACTERISTICS OF ACTUATORS 35 


2.9.3 Mixed motorization 


In practice, it is observed that there exists a whole range of intermediate solutions which attempt 
to make the best compromise between both extreme solutions. 


Let us also note that a significant effort is currently made to produce efficient direct drive 
actuators of electric type, in which case distributed motorization will become the most relevant 
choice. 


2.10 Mechanical characteristics of actuators 


A robot actuator is a device able to generate a force or a torque at variable speed, and thus 
capable to modify at any instant the geometric configuration of the mechanical structure. 


If we limit ourselves to the types of actuators that can be used in robotics, it is possible to 
classify them according to the following criteria: 


e The type of motion generated: with the present technology, it is possible to use linear 
actuators which develop a force and generate a translation motion in the same direction, 
and rotary actuators which develop a torque and generate a rotation motion about the 
torque axis; 


e The nature of the primary source of energy: one disposes of 


— pneumatic actuators, whose source of energy is compressed air; 
— hydraulic actuators, which develop their power from oil under pressure; 


— electrical actuators. 


'The power to mass ratio and the acceleration power are two important criteria to make an 
objective comparison of the different actuator types. 


2.10.1 Power to mass ratio 


The power to mass ratio of an actuator is defined as the ratio of the mechanical power developed 
by an actuator to its mass. Typical values that can be reached are 


e For an electric actuator, 0.5 to 0.6 kW/kg; 
e For a hydraulic actuator, 2.5 to 5 kW/kg. 


'The power to mass ratio of hydraulic actuators is thus from 5 to 10 times better, and furthermore 
hydraulic actuators can give an efficiency higher than 5096 since they generally allow to drive 
the joints directly. 


Hydraulic energy provides thus very compact actuators, perfectly well adapted to the needs of 
a distributed motorization. Hydraulic actuators fit well within a general architecture of the 
manipulator of simple open-tree type and allow for a certain modularity in the design (which 
means that a given arm can be made from an assembly of standardized elements providing each 
the mechanical structure and the actuation of one DOF). One has however to mention that 
there is a need for small power hydraulic actuators as required in light duty robotics such as 
electronic assembly. 
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Figure 2.10.1: Model of a 1 DOF mechanical transmission 


The pneumatic actuator would benefit very much of the same properties as hydraulic ones if 
on one hand, the alimentation pressure was not limited to about 15 bars approximately, mainly 
due do constraints arising from workshop environment, and if on the other hand the air was not 
characterized by a high compressibility which makes pneumatic feedback technology difficult. 
Therefore, pneumatic actuators are only used for small robots of “pick-and-place” type and for 
robots used in deflagrating environment. 


Due to their relatively power to mass ratio, electric actuators are better adapted to a centralized 
motorization, or at least to a motorization where the actuator locations are chosen in the 
kinematic chain at a certain distance from the driven joints. A compromise has to be found 
between lightening of the structure through location of actuators in its less mobile parts (such 
as the waist and the upper arm) and the mechanical complexity arising from the passage of 
transmission chains through joints located up-stream in the mechanism. It is generally observed 
that the use of electric actuators leads to designs with complex kinematic architecture (and thus, 
high structural stiffness) and high level of integration (hence, with no or very little modularity). 


2.10.2 Maximum acceleration and mechanical impedance adaptation 


Reduction devices are present in many robotic and mechatronic systems. Indeed motor charac- 
teristics are not always adapted to the desired motions. At first the velocity of the load have 
to be generally much smaller than the rotation speed of electric motors to achieve the desired 
precision values. Secondly most of the actuators are rotary motors, while the load is motion is 
often linear. So a mechanism to transform the rotation motion into a linear motion is necessary. 
Finally as most of electric motors have high speed their torque is low, so they provide low torque, 
which is not good. Therefore a reduction the velocity is also mandatory. 


Modeling of a 1 DOF reduction system 


'The proper choice of a system transmission ratio is necessary to produce maximum system 
acceleration. In order to illustrate the concept of adaptation of mechanical impedances, one 
considers the model of 1 DOF mechanical system consisting of an inertial load driven by a 
motor through a mechanical transmission (Figure 2.10.1). One defines: 


- Wm, the velocity of the motor shaft, 
- We, the velocity of the load, 
- Jm, the internal inertia of the actuator, 


- Je, the inertia of the driven load, 
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- Ym, the torque delivered by the actuator, 
- Yc, the torque that is absorbed by the load. 


The reduction ratio r of the transmission device is defined as the ratio of the output velocity to 


the input velocity: 
r= mm (2.10.1) 


We 


For gear pairs, this ratio can be also expressed in terms of the number of teeth of the two wheels 


(see Figure 2.10.1) 
Z2 
=> 2.10.2 
r= (2.10.2) 
One can generally neglect the internal frictions of the motor but not the efficiency of the trans- 
mission device. This efficiency ration is defined in terms of the input and output power of the 


reduction system. 


pee (2.10.3) 


Y1 Wm 


where one defines: 
- 71, the resistant torque that is presented by the transmission device to the actuator, 
- y2, the torque that comes out from the reduction device to drive the load. 


The efficiency ration is generally not well known. This value is defined for steady state regimes 
and given standard conditions, but generally the system is used in different conditions. Nonethe- 
less it makes sense to use this efficiency ratio for instantaneous power in transient regimes even 
under non standard conditions. To account for the approximation, it is necessary to take a quite 
large security margin: a security margin of 2596 is often a good guess. Introducing the reduction 

ratio r, the input and output torques are related by: 
gp eee (2.10.4) 

TT) 


'The dynamic equilibrium of the motor shaft and of the load shaft leads to: 


dwm 
m — = m — 2.10.5 
di Y V1 ( ) 
dwe 
5 = — Ye 2.10.6 
dt Y2 Y ( ) 


Eliminating the intermediate variables qı and y2, one gets the equations of the dynamic equi- 
librium of the system: 


jm 7o mda 


'The dynamic equilibrium can be expressed in terms of the acceleration of the motor or of the 
load: 


Ye Je dwm 
m = =£ J) a 2.10.7 
L m ae dt 
c Je d c 
EEE (= + rn) = (2.10.8) 
rn rn dt 


Equations (2.10.7) or (2.10.8) are useful for many design problems. An important design problem 
consists in determining the torque to operate the system to move a given load with prescribed 
speed and acceleration. 
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Mechanical impedance adaptation problem 


When the load torque ye and the maximum acceleration of the load dw./dt are given, there is a 
value of the reduction ratio that minimizes the torque to be delivered by the motor. The value 
of the actuator torque in terms of r is given by the equation (2.10.8). The optimal value of the 
reduction ratio is the one for which the derivative of the y,, with respect to r is equal to zero, 


that is: 
dwe 1 dwe 
Jm — — —— | Je +y] = 0 
dt = rn ( dt us ) 
One gets the optimal value of the reduction ratio: 


Je Ge + Yc 
* = TL (2.10.9) 
Im Ge 


In the expression of ym, one can distinguish two terms: The first one (rJ, dosc) depends linearly 
on r, while the second one (E + A des) depends on 1/r. An interesting property of the optimum 


is that the two the terms become equal and one has: 


dwe 
Ym = Ms (2.10.10) 


When there is no resistant torque (i.e. ye = 0) and the reductor is ideal (i.e. 7 = 1), it 
comes: 


J, 
* — e 2.10.11 
EET. (2.10.11) 

This can also be written: r 
Jim 2 (2.10.12) 


This means that the design torque is minimum when the inertia of the load reported at the 
actuator is equal to the inertia of the motor. 


Optimal reduction ratio for maximal acceleration of the load 


Another interesting problem is to find the reduction ratio that maximises the acceleration of 
the load for given values of the motor and resistant torques ym and ye. Equation (2.10.8) gives: 
d mE 

EM ea —À. (2.10.13) 
dt 2 TrJa 


'The optimality conditions of this problem is obtained by expressing that the first derivatives 
of the acceleration of the load is zero: 


d de, PE r4)- Om — EFE H Im) R 
dr dt — (2 +rJm)? 7 


After some algebra one finds that the optimal reduction ratio r* is the solution of the quadratic 


equation: 
"Ime ea i germ 1 
n T n r 
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It comes: 
l 9e Jm y32J2 Jm 


rs Ym Je 72, J2 US 


(2.10.14) 


So the general solution of the problem of impedance adaptation given by (2.10.9) and of the 
maximization of the acceleration power given by (2.10.14) are different. However they are equal 
when the transmission system is ideal. 


Let's consider the ideal problem again, that is when there is no resistant torque (i.e. ye = 0) 
and the reductor is ideal (i.e. 7 = 1). The acceleration of the load is: 


dwe Ym 


—— = L———— 2.10.15 
dt de +rdJm ( ) 
The optimal reduction ratio satisfies : 
d dwe Ym (72$ + Jm) 
— Ooo eee] t em 0 
dr dt (22 + rm)? 
This equation has the same solution as for the minimal torque problem: 
J 
* = z 2.10.16 
MESE (2.10.16) 


Acceleration power of actuators 


For hydraulic actuators which develop the torque Ym at low velocity, the transmission ratio can 
be very close to 1. They have thus a very high acceleration power which makes them specially 
well adapted to robots from which high accelerations are required. 


The acceleration power of pneumatic actuators gives them characteristic properties close to 
those of hydraulic ones as far as dynamic response is concerned. 


On the other hand, electric actuators provide a low torque with high rotation speed: they 
have thus to be used with high transmission ratios, and it is impossible to realize the optimal 
reduction ratio gven by equation (2.10.16) that gives the highest acceleration to the load. Thus 
their acceleration power remains limited. 


Rotation to translation transformation 


The approach developed in the previous section can easily be extended to various of transmis- 
sion devices. For instance, the rotation to translation transmission system (helicoidal pair) is 
presented here after. 


Let us define 
- T, the torque or force developed by the actuator, 
- J, the internal (rotary or translational) inertia the actuator, 
- M, the inertia of the load driven by the actuator, 


- r, the transmission ratio defined as the ratio of output to input (angular or linear) velocity, 
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Figure 2.10.2: 1 DOF model of mechanical transmission 


- a, the output linear acceleration of the system. 


By the very definition of the transmission ration one has the following relationship between the 
displacement x of the load and the angular displacement 0 of the motor 


x — rÜ with r= = 
27 
where p is the advance length per rotation. Thus for accelerations one has: 
m a 
g=- 
m 


One has also conservation of the work through the transmission device: 
C0 = Fa or C=rF 


Therefore when seen from the motor side, the inertia force of the load is 


C = rMa 
Hence the dynamics of the system is governed by the equation 
J 
T = (— + Mr)a (2.10.17) 
r 
and the resulting acceleration is 
Tr 
z — 2.10.18 
= 7+ Mr ( ) 
It takes a maximum value when the transmission ratio is such that 
da T 2r? T M 
— = —_ - — = 0 2.10.19 
dr J + Mr? (J + Mr?)? ( ) 
or 
J 
opt = 4| — 2.10.20 
T pt M ( ) 
'Therefore, the acceleration power of the actuator is given by the formula 
1 T 1 T 
(2.10.21) 


amar = FS = 72 F 
2/MJ 2 ropt M 


Equation (2.10.21) provides the value of the maximum acceleration that an actuator can com- 
municate to a given load M when developing a starting torque T. 
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2.11 Different types of actuators 
'The ideal characteristics that are expected from a robot actuator are: 
e A low inertia in order to increase the acceleration power of the joints; 


e A high mechanical stiffness in order to minimize the deflection at tool level under the 
action of the load; 


A low operating speed Vo: a few rad/s in rotary motion and a few tenths of cm/s in linear 
motion in order to obtain tool velocities from 1 to 5 m/s; 


A velocity range with continuous variation from —Vo to -- Vo; 


A high output force even at zero velocity, in order to provide an adequate holding torque; 


Low non-linearities (dry friction, backlash, etc.) 


The possibility to perform velocity and/or force control. 
No actuator currently available fulfills all the conditions mentioned here above. The most 
suitable actuators are: 

e Among the electric actuators: 


— The step motors, 


— The direct current (D.C.) motors with constant inductor flux (generated either by 
a permanent magnet or through a constant inductor current) and controlled by the 
armature current; 


e Among the hydraulic actuators: 


— 'The linear and rotary pistons, 
— The rotary motors with axial pistons; 


e Among the pneumatic actuators: 


— The linear and rotary pistons. 


2.11.1 Step motors 


The principle of step motors is to convert directly a electric digital signal into incremental 
angular positioning: the step motor driver, which plays the interfacing role between the control 
unit and the motor, receives clock pulses at a varying frequency, each pulse generating an angular 
displacement of fixed amplitude called the angular step. 


The average rotation speed Nm, expressed in rpm, is equal to 


Nise Ud. (2.11.1) 


n 


where 


- f is the frequency of clock pulses 
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- n is the number of steps per revolution of the motor (usually, from 200 to 400 steps per 
revolution). 


This relationship remains verified under limited load and acceleration/deceleration conditions 
above which positioning errors may occur. 


Due to its very principle, step motors are velocity controlled, and their main advantage is to 
avoid the need of a closed-loop servo system since the revolution angle is proportional to the 
number of clock pulses received. 


Step motors can be of three different types: 
e Motors with permanent magnet armature, 
e Motors with variable reluctance, 
e Hybrid motors. 


The last ones gather the advantages of both former categories and are thus the more performing. 
They develop a relatively low torque, which can however be hold at zero velocity, and their 
internal inertia is relatively high, so that they have a low power to mass ratio. 


2.11.2 Direct current motors 


In robotics, the most used type of D.C. motor is the armature-controlled one with constant 
inductor flux. It possesses a relatively high operating speed (from 2000 to 3000 rpm) and 
implies thus a transmission chain with high reduction ratio which is a source of backlash and 
reduces the acceleration power. 


D.C. motors can be of different types: standard motors in which the armature is wound to a 
magnetic material, bell motors in which the armature conductors are attached to an insulated 
cylinder and disk motors in which the armature conductors are attached or wound to an insulated 
disk. Among these, the disk motor possesses the smallest internal inertia, but still has a relatively 
low acceleration power. 


'The models needed to control D.C. motors are well known: when they are voltage-controlled, 
the transfer function rotation speed versus input voltage is of second order but can be reduced 
to a first order one under certain approximations. When current-control is used, the transfer 
function rotation speed versus input current is of first order and the torque developed is then 
proportional to the input current (if friction effects are neglected). 


D.C. motors can thus be either velocity - or torque - controlled, and the appropriate drivers are 
servo-amplifiers and pulse width modulation - amplifiers. 


2.11.3 Hydraulic actuators 


Pistons are very simple and effective hydraulic systems which exist in either linear or rotary 
form. In their linear version, the displacement range is generally of a few centimeters or dizains 
of centimeters, while in the rotary version the angular displacement is limited to about 270 deg. 


Axial piston motors provide a continuous rotation but their technology is significantly more 
complicated. 
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'Their control is achieved either through a small electric motor acting on a distributor or using 
a servovalve which provide the interface between the control unit and the actuator itself. 


'The main advantages of hydraulic actuators are, as already mentioned, their low internal inertia 
and high acceleration power. They are also characterized by a high mechanical stiffness. 


However, they exhibit a large number of nonlinearities of different origins which make the 
associated feedback system difficult to stabilize. 


'The ordinary pistons do not raise significant problems when maintained at zero speed, but this 
is not the case of axial piston actuators which present important torque and velocity oscillations 
near zero speed. 


2.12 The sensors 


Our goal in this section is not to make a complete review of all the sensors used in robotics. We 
are only interested in the proprioceptive sensors which provide information on the current state 
of the mechanical structure and thus, allow us to control its motion. Controlling the motion of 
the manipulator implies having a system allowing us to know at any instant the configuration 
parameters of the system and their first order time derivatives. In this way, information is 
always available on position and velocity for all the members. 


Two types of sensors have to be considered: 
e position sensors, 


e velocity sensors. 


Let us note that we will not consider the most elaborate category of sensors which measure 
directly the location of the tool and its velocity in a 3-D space, for the reason that they still 
make the object of intensive research and are not yet well developed. 


2.13 Integration of sensors in the mechanical structure 


The role of the sensor in the feedback loop is to measure either the position or the velocity of 
the joint whose displacement constitutes the configuration variable. 


Integration of the position and velocity sensors in the power transmission chains linking the 
actuators to the joints can be made in two ways. 


e solution 1: direct measurement The first choice corresponds to the case where the sensor 


is attached to the joint and measures thus directly its displacement: it is a theoretical 
situation. 


The accuracy on the measurement of the configuration variable is then equal to the res- 
olution of the sensor. However, all the imperfections present in the design of the joint 
and its transmission, if they do not alterate the measurement itself, affect both the ac- 
curacy and stability of the feedback system. Therefore, it is important to minimize the 
structural compliance, the friction and the backlash in the transmission. It has unevitably 
consequences on the cost of the design. 
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Figure 2.13.1: Direct measurement of joint position and velocity 
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Figure 2.13.2: Indirect measurement of joint position and velocity 


Note also that making the measurement directly at joint level implies the availability of 
sensors designed to measure low velocities. 


e solution 2: indirect measurement 


The alternative solution consists of attaching the sensor to the actuator axis, in which case the 
result of the measurement has to be multiplied by the transmission ratio r as defined in section 
2.11 in order to provide the value of the configuration parameter. 


To provide the same accuracy in the measurement, the resolution of the sensor does not need to 
be as good as in the former case since the measured quantity is multiplied by the transmission 
ratio. However, the quality of the measurement is now affected by the loss of accuracy in the 
transmission. 


2.13.1 Position sensors 


Figure 2.13.3 summarizes the different methods to perform position measurements which we 
will briefly review: 


e digital position sensors 
e incremental encoders 


The principle of incremental encoders (Figure 2.13.4(a)) is to generate a pulse for any variation 
of the configuration parameter equal to the basic step of the encoder. Incremental encoders 
have thus to be associated to an algebraic counting device which sums with their correct sign 
the pulses generated by the encoder. The counter contains at any instant the value of the 
displacement made from the reference configuration in which it has been reset to zero. 


e Absolute digital encoders: 


They provide a binary code information which corresponds to the current value of the 
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Figure 2.13.3: The various methods for measuring joint positions 


Figure 2.13.4: Digital sensors: (a) incremental encoder (b) absolute encoder 
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Figure 2.13.5: Inductive potentiometer 


configuration parameter. Compared to incremental encoders, they are much more difficult 
to construct (figure 2.13.4(a)). 


Thermal dilatation is a serious problem for linear encoders. One prefers thus to use 
rotary encoders since angular values are insensitive to temperature. It is possible to find 
rotary encoders of incremental type numbering from 40 to 40000 steps/turn, and absolute 
encoders providing an information (on one turn) encoded on 6 to 16 bits. 


The accuracy obtained with digital sensors is quite comparable to that given by analog 
sensors. For similar accuracy, incremental encoders are much cheaper than absolute ones. 
However, due to their incremental principle, they loose the positioning information when 
the system is shut down. It is then necessary to reinitialize the reference position of the 
robot. 


Analog position sensors 


Analog position sensors integrate in the same design the sensor itself which delivers an 
analog signal proportional to the measured position and the analog-to-digital converter 
which provides a digitized value. Three types of analog position sensors are widely used 
in robotics: 


1. Resistive potentiometers: 


Of linear or rotary type, they convert the cursor position into a proportional D.C. 
tension. A analog-digital converter converts then this tension into a digital signal. 
The resistive potentiometers made of a conducting plastic track have a quasi-infinite 
resolution, their linearity is excellent (0.1%) and their resistance to wear is very high. 
They are widely used because of their relatively low cost. 


2. Inductive potentiometers: 


They consist of a transformer with variable coupling which delivers a tension pro- 
portional to the displacement of the mobile winding. They exist in rotary version 
as shown by figure 2.13.5 and in linear form. The analog-to-digital converter has 
to achieve the demodulation of the output signal before transforming it into a form 
suitable to the control system. A linearity of about 0.1% is obtained on a limited dis- 
placement range (a few cm in translation and a few tens of degrees in rotary motion). 
Their use is thus limited to short displacement ranges. 
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2.13.2 


Figure 2.13.6: Principle of the resolver 


3. Resolvers: 


They are also transformers with variable coupling, and they exist in both linear and 
rotary versions. The stator is made of two coils as represented by figure 2.13.6, 
and both are fed with alternative voltages set at 90 deg of each other. They deliver 
thus at the output of the mobile winding a voltage with a phase (measured from a 
given reference) proportional to the displacement. Since they are based on a phase 
measurement, the associated converter is relatively complicated. It delivers either 
directly one digital signal proportional to the phase or two signals giving both cosine 
and sine of the phase angle. 


Although any analog sensor device is based on the measurement of an continuous 
signal, in all cases a analog-to-digital converter digitizes the signal measured. In 
practice, the digitalization is performed on 10 to 12 bits. This choice corresponds to 
an optimum since a finer digitalization would generate too much noise in the signal 
fed to the control unit. 


Velocity sensors 


Figure 2.13.7 summarizes the different methods to measure velocities on a robot arm. If we 
except the measurement of velocities using a tachometer, all the systems generally used are 
based on the use of a position sensor of incremental type. 


e velocity measurement by pulse counting 


Since velocity is equal to displacement divided by time, there are two ways of measuring 
them. One can either measure the delay between two successive pulses, or count the 
number of pulses occurring during a specific interval. Both methods have their advantages 
and drawbacks 


e The encoder pulse counting method 


A counter is active during a fixed time interval t and counts the number of pulses generated 
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Figure 2.13.7: The various methods for measuring joint velocities 
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Figure 2.13.8: Velocity measurement by encoder pulse counting 
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Figure 2.13.9: Velocity measurement based on clock pulse counting 


by the encoder during that period (figure 2.14.8). The measured velocity (in m/s or rad/s) 
is then given by the formula 


ee (2.13.1) 
pt 
where 


— n is the number of pulses detected; 
— p is the number of steps (per meter or per radian) of the incremental encoder; 


— tis the time interval. 


The relative error affecting the measurement is given by 


LAE SEL OL (2.13.2) 
V n t p n 


since p is known exactly and A is negligible by comparison with An, Moreover, since An 
is equal to 2, the relative error on V is inversely proportional to n. The measurement has 
thus increasing accuracy with n. To increase n is equivalent to 


— Using an incremental encoder with a large number of steps per meter or radian, 
— Measuring large velocities, 


— Increasing the counting time t, which however alters the instantaneous character of 
the measurement since it has then the meaning of an average velocity over t. 


In practice, with the encoder pulse counting method the sensor has to be mounted on the 
actuator rather than at the output of the transmission device. 


e The clock pulse counting method 


A counter, validated during the time interval occurring between two successive pulses of 
the incremental encoder, counts the pulses emitted by a high frequency clock (figure 2.13.9) 
The measured velocity (in m/s or rad/s) is then given by 


V om (2.13.3) 


in which 


— n is the number of pulses detected; 
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Figure 2.13.10: Velocity measurement based on averaging 


— f is the clock frequency; 
— pis the number of steps per meter or per radian of the encoder. 


Again, the relative error is 

AV An A A An 
V n f p n 
since the clock frequency and the number of steps per length unit are exactly known. This 


relationship shows that the measurement accuracy improves with greater n, which can be 
obtained by 


(2.13.4) 


— Using an incremental encoder with small number per length unit, which is a limitative 
procedure since the measurement looses its instantaneous character; 
— Increasing f, but there is a technological limit to the clock frequency; 


— Measuring slow velocities, which can be obtained by mounting the incremental en- 
coder on the joint axis. 


Analog velocity measurement through averaging 


Let us suppose that the pulses generated by an incremental encoder are perfectly calibrated 
both in amplitude A and in duration Tp (figure 2.13.10) If T is the time elapsed between 
two successive pulses, the average value of the output signal Em on the period T is equal 
to 


ATo 
En = — 2.13.5 
7 (213.5) 
On the other hand, the velocity can be evaluated by 
1 
V = — 2.13.6 
-T (2.13.6) 


where p is the number of steps per unit length of the encoder. After elimination of the 
period T between equation 2.13.5 and equation 2.13.6, one obtains a relationship between 
velocity and the average value of the output signal 
Em 
V = 2.13.7 
»AT; ( ) 
Velocity is thus proportional to the average value of the output signal which can be gen- 
erated through a low-pass filter. 


Let us note that the physical condition Em < A has the consequence that the maximum 


measurable velocity is equal to Tm 
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e Analog velocity measurement using a tachometer 


A tachometer is a D.C. machine which provides a D.C. tension proportional to the rotation 
velocity of its armature. Its sensitivity is generally of the order of a few volts per 1000 
rpm. 


Its use is thus reserved to the measurement of relatively high velocities and it has to be 
mounted on the actuator axis. 


It has good linearity properties (~ 10~%), but its use remains limited for cost reasons. 


It has to be associated with an analog-to-digital converter in order to provide a measure- 
ment into suitable digital form for further treatment by the control unit. 


2.14 The robot manipulator ASEA-IRb-6 


In order to illustrate the preceding considerations, let us consider as an example the robot 
manipulator ASEA-IRb-6 represented by figure 2.14.1 It possesses a mechanical architecture 
in which the main kinematic chain is of RRRRR type. Its wrist possesses only 2 DOF in the 
standard version. The five degrees of freedom correspond to: 


eR 


. the waist rotation ~ producing arm sweep, 
the shoulder rotation ¢ producing upper-arm motion,, 
the elbow rotation o producing lower-arm motion,, 


the wrist bending t, 


QU amho GARS 


the wrist twist v. 


The manufacturer has opted for the choice of electrical motorization (servo-controlled D.C. 
motors) centralized in the basis. Therefore, the different DOF are actuated through secondary 
kinematic chains parallel to the main one. 


The displacement ranges of the successive DOF and the associated velocities are: 


waist rotation 340? 95 ?/s 

shoulder rotation +40° radial velocity of 0.75m/s 
elbow rotation 25? vertical velocity of 1.1m/s 
wrist bending +90° 115°/s 


wrist twist +180° 195°/s 


The corresponding workspace is represented hereafter in perspective view on one hand, and 
through a vertical plane cut on the other hand. The manufacturer does not specify the accuracy 
of the machine, but its repeatability is 0.2mm. A certain number of mechanical characteristics 
are given by the technical sheet provided at the end of the chapter. 


Mechanical design 
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Figure 2.14.1: Mechanical design of the robot ASEA IRb-6 
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Figure 2.14.2: Workspace of the ASEA robot IRb-6: perspective representation 


Figure 2.14.3: Workspace of the ASEA robot IRb-6: vertical cut representation 
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Figure 2.14.4: Motor unit for waist rotation 
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Figure 2.14.5: Four bar mechanisms for motion transmission 


Figure 2.14.4 displays the mechanical design of the first DOF: the whole motor unit is 
rigidly attached to the base of the manipulator and generates rotary motion of the base through 
a gear reductor. The tachometer provides velocity information and the resolver, positioning 
information. The motor units for the DOFs ¢ and a (shoulder and elbow rotations) are also 
localized in the base, the motion transmission being achieved through four bar linkages which 
constitute the most elementary closed-loop mechanisms (figure 2.14.5). Figure 2.14.6 displays 
the motor unit of the shoulder: the actuator rotary motion is transformed into translation motion 
through a ball screw whose nut acts on a lever hinged to the arm. In this way, a rotation-rotation 
transformation of motion with a high reduction rate is obtained while allowing us to locate the 
motor unit of the shoulder in the base of the manipulator. The ball screw is a very efficient 
implementation of the screw joint (figure 2.14.7). It is made of a helicoidal screw whose thread 
is a rolling path for the balls and a nut containing them. The balls are recirculated from one 
end of the nut to the other through external ducts. 


The motor unit of the elbow is designed on the same principle (figure 2.14.8). An additional 
mechanical loop is however needed to transmit the motion to the end of the arm. 


'The motor units for wrist bending and twist are also located in the base of the structure, and the 
motion transmission from the base to the wrist is obtained through a parallelogram mechanism, 
as shown by figure 2.14.9 The transmission is made of three quaternary links having the form 
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Figure 2.14.6: Motor and transmission unit for shoulder rotation 
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Figure 2.14.7: Ball screw implementation of the screw joint 
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Figure 2.14.8: Motor and transmission unit for elbow rotation 
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Figure 2.14.9: Transmission unit for wrist bending 


of disks and located at motor, elbow hinge and wrist levels, respectively. They are connected 
together through parallel links which are hinged on the disks at points distant from 90°. Two 
similar linkages going through the main structure are necessary to transmit both bending and 
twist DOF (figure 2.14.9). For the wrist DOFs, the speed reduction is achieved first through 
gear reductors of harmonic drive type. Harmonic drives are highly performing reductors based 
on a rather simple but clever principle. They are made of three parts: 


e the output element is a rigid planetary of annular shape with internal teeth ; 
e the input element is a elliptic wave generator mounted on a roller bearing of same shape; 


e the intermediary element is a deformable satellite, with a number of teeth slightly inferior 
(generally, 2) to that of the planetary. 


One can show that, if Zp and Z, are respectively the numbers of teeth on the planetary and on 
the satellite, the reduction ratio is given by the formula 


Zp 


N = 
AEA 


(2.14.1) 
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Figure 2.14.10: Reductor of harmonic drive type 


In this way, reduction ratios as high as 320 can be obtained in a single reductor stage. The 
number of teeth participating in the contact is rather high (about 10 %); this is favorable to 
wear and backlash reduction and increases at the same time the mechanical stiffness. 


2.15 Technical sheets of some industrial robot manipula- 
tors 


Starting from the technical data provided by manufacturers, it is generally possible to summarize 
the characteristics of a specific manipulator in a technical sheet made on the same model as the 
ones given hereafter. However, it is not always possible to gather all the data which describe 
completely the manipulator under consideration. 
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2.15.1 Industrial robot ASEA IRb-6/2 


KINEMATIC ARCHITECTURE: 5R 


DEGREES OF FREEDOM: 
a R 340 90°/s 

q2 80° 1m/s 

q3 50° 1.35m/s 
q4 180°  90°/s 

ds 360"  150/s 


MOTORIZATION: D.C. servomotors 


MECHANICAL PERFORMANCES: 

capacity: 6kg 

max. linear velocity: r direction 0.75 m/s; z direction 1.1m/s. 
repeatability: 0.2 mm 

accuracy: not specified 


EFFECTOR: various grippers adapted to specific applications 


SENSORS:  resolvers + tachometers 


CONTROL UNIT: central unit with 16K ram 


PROGRAMMING MODE:  - teach box 


- computer link 


APPLICATIONS:  - arc welding - assembly 
- inspection - machining 
- surface coating - glueing 


Industrial robot ASEA IRb-6/2: technical data 
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Kinemattic architecture 


Workspace representation 


Figure 2.15.1: Industrial robot ASEA IRb-6/2: geometric configuration 
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Kinemattic architecture 


Workspace representation 


Figure 2.15.2: Industrial robot SCEMI 6P-01: geometric configuration 
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2.15.2 Industrial robot SCEMI 6P-01 


KINEMATIC ARCHITECTURE: 6R 


DEGREES OF FREEDOM: 
qı 268° 233^/s 
" 120? — 233*/s 
q3 116? 233° /s 
qa 360? — 233^/s 
qz 250?  1089/s 
d 360°  233^/s 


MOTORIZATION: D.C. servomotors 


MECHANICAL PERFORMANCES: 
capacity: 1.5kg 

max. linear velocity: 2.7m/s. 
repeatability: 0.04 mm 

accuracy: not specified 


EFFECTOR: standard gripper (electrically or pneumatically actuated) 


SENSORS: incremental encoders (1000 and 800 steps/turn) 


CONTROL UNIT: central unit LSI - 11/2 with 64K ram 
1 microprocessor per axis, position/velocity control 


PROGRAMMING MODE: - teach box 
- programming language LM 
- via computer link, language RS 232 — C 


APPLICATIONS:  -flexible manufacturing - assembly 
- inspection - palletizing 
- component insertion - research and teaching 


Industrial robot SCEMI 6P-01: technical data 
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Kinemattic architecture 


Workspace representation 


Figure 2.15.3: Industrial robot PUMA 560 Geometric configuration 
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2.15.3 Industrial robot PUMA 560 


KINEMATIC ARCHITECTURE: 6R 


DEGREES OF FREEDOM: 
qi 320°  809/s 
qo 250° 5 2°/s 
q3 270° 108°/s 
qa 300° 230°/s 
qs 200? 108°/s 
do 360? — 453^/s 


MOTORIZATION: D.C. servomotors 


MECHANICAL PERFORMANCES: 
capacity: 2.5kg 

max. linear velocity: 1.0m/s. 
repeatability: 0.1 mm 

accuracy: not specified 


EFFECTOR: standard gripper (optional, pneumatically actuated) 


SENSORS: incremental encoders (0.73 to 1.1710 ^ rad/bit) 


CONTROL UNIT: central unit LSI - 11/2 with 16 or 32K ram 


1 microprocessor per axis 


PROGRAMMING MODE:  - teach box 
- programming language V ALT M 
- via computer link, language V AL IITM 


APPLICATIONS:  - arc welding - assembly 
- inspection - machining 
- component insertionsearch and teaching 
- palletizing 


Industrial robot PUMA 560: technical data 
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Kinemattic architecture 


Workspace representation 


Figure 2.15.4: Industrial robot ASEA-IRB 1400 Geometric configuration 
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2.15.4 Industrial robot ASEA IRB1400 


KINEMATIC ARCHITECTURE: 6R 


DEGREES OF FREEDOM: 
qı +150° 110°/s 
q2 +70° 110°/s 
da 70° — —65° 110°/s 
q4 +150° 280°/s 
q5 +115° 280° /s 
qe 300° 280? /s 


MOTORIZATION: D.C. servomotors 


MECHANICAL PERFORMANCES: 

capacity: max 5.kg 

max. linear velocity: 1.0m/s. 

repeatability: 0.1 mm (during unidirectional pose) 
accuracy: 0.2mm (during unidirectional pose) 
stabilization time: « 0.15s 


EFFECTOR: 


SENSORS: 


CONTROL UNIT: central unit LSI - 11/2 with 4 Mbyte (basic) RAM 
extended to 10 Mbyte 


PROGRAMMING MODE: - RAPID programming language 


APPLICATIONS:  - flexible manufacturing - research and teaching 
- arc welding - assembly 


Industrial robot IRB-1400: technical data 
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Figure 3.1.1: A typical assembly task 


3.1 Objectives of robot control 


When considered at a higher level, the objective of robot control is to let it achieve a specified 
task. Let us consider an elementary assembly task such as suggested in figure 3.1.1. It consists 
in picking up a cylindrical pin A on a conveyor and inserting it in a hole made in object B. 


It can obviously be decomposed in a sequence of elementary orders such as 


< MOVE... >; 
< GRASP >; 

< MOVE... >; 
< RELEASE >; 


in which case each motion phase imposes the effector to describe an elementary and well specified 
operation. 


3.1.1 Variables under control 
Achieving a task such as described above implies to know at every moment: 
- the position, orientation and state (open/ closed) of the gripper; 


- the position and orientation of pin A, which depends itself on the instantaneous position of 
the conveyor; 


- the motion of the conveyor; 
- the position and orientation of object B; 


- the position and orientation of the hole relatively to it. 
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Operational Space > Configuration 
x-f(0) Space 
n < To 
Direct Problem 


Figure 3.1.2: Fundamental relationship between operational and configuration spaces for a robot 
manipulator 


All these geometric data are generally know in different reference frames. In order to have 
global control of the task, all of them have to be expressed in a common reference frame which 
defines the operational space, or task space. 


The sequence of successive configurations of the effector in the operational space defines its 
trajectory. It can be written in terms of a certain number of position, orientation and state 
parameters functions of time. 


x(t) = [21(t)...an(t)]” 


Each configuration of the effector is obtained through an appropriate modification of the ge- 
ometrical configuration of the articulated mechanism by acting on the m joint (or configuration) 
variables. 


e(t) = [01(t)...Om(t) ]” 


which define the joint (or configuration) space. From a geometrical point of view, controlling 
the motion of the robot consists in verifying in time the n relationships: 


x1(t) = fi(@i, — 
x(t) = f2(01, "mm 


Talt) = — fua(01,...05,) 


which establish the correspondence between the operational space and configuration space. They 
can be expressed in the matrix form 


x(t) = f0) (3.1.1) 

which constitutes the fundamental relationship for kinematic analysis of robots (Fig. 3.1.2). 
As will be seen, equation (3.1.1) always be verified and has a unique solution when cal- 
culating a from given 0. This is the direct problem of kinematics. However, the solution of 
the inverse problem x — 0 does not necessarily exist and, if it does, can be multiple or even 


in infinite number. Developing the solution of the inverse problem is thus one of the difficult 
aspects of robot control. 


3.1.2 Robot motion control 


'The process of motion generation in joint space is then the following: 


e joint motion 0(t) results from the action of couples C(t) which are developed in the 
articulations; 
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Figure 3.1.3: Three main levels of robot control 


e the couples C(t) result themselves from torques F(t) generated by the actuators and 
transmitted to the articulations by transmission devices (gear trains, mechanical linkages); 


e the actuators produce the torques F(t) in response to current or voltage orders sent by 
the control unit and assembled in a vector V (t). 


As a result, controlling robot motion consists essentially in controlling the bi-directional 
equation. 


V(t) o F(t) o C(t) e 0(t) ^ a(t) © task (3.1.2) 


Three levels of robot control can be distinguished in equation (3.1.2) which are relevant from 
different disciplines: 


e level 1 which is the artificial intelligence level, 


e level 2 or the control mode level, 
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e level 3 or the servo control level. 


Each of them will be briefly described (see Figure 3.1.3). 


3.1.3 Level 1 of control: artificial intelligence level 


'The objective of this first and highest control level consists in translating the sequence of orders 
sent to the control unit into a trajectory information a(t). 


Its degree of evolution depends on the point of view adopted. Coming back to the typical 
task of figure 3.1.1, a sequence of orders such as 


< PICK A>; 
< INSERT A INTO B >; 


is a possible and very concise way of programming the objective. However, its achievement 
implies solving a certain number of problems which are relevant of artificial intelligence such as: 


e Interpreting the natural language in terms of which the orders are expressed; 
e Perceiving the state of the robot and its environment; 

e Describing the task in terms of elementary operations; 

e Planning the motion and generating the trajectories. 


This highest level of control is still largely a research subject and is this reduced to its 
simplest expression in current industrial practice. The normal programming procedure of present 
industrial robots is to give directly the elements concerning the trajectory (either in configuration 
space 0 or in operational space x) during a training phase. 


Motion planning and trajectory, generation are the subjects by which the mechanical engineer 
is the most concerned in this highest level of robot control. 


3.1.4 Level 2 of control or the control mode level 


'This is the level at which the bi-directional relationships between the trajectory in operational 
space a(t) and the torques F(t) supplied by the actuators can be verified. 


It is important to notice that several modes of control can be imagined. The variety in the 
proposed solutions arises from the fact that the relationship 


a(t) ^ 6(t) > CE) F(t) (3.1.3) 


pose various problems in practice. They differ mainly by the level of knowledge of system that 
one accepts to integrate in the model. 


In particular, 
e The relationship F(t) C(t) involves an appropriate model of transmissions; 


e Controlling the relationship C(t) — 0(t) implies using a dynamic model of the articulated 
structure; 


e Controlling the relationship 0(t) e a(t) relies upon a kinematic model of the mechanism. 
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(b) Velocity control 


Figure 3.1.4: Comparison of the principles of position control and velocity control 


Among these various models, the dynamic one is the most difficult to build and use for 
mainly two reasons: 


e There is no way of knowing how to model the imperfections of the mechanical components 
correctly (mainly flexibility, friction and back-lash in joints); 


e Even if it seemed possible to take these into account, the model would include hundreds 
of parameters and the processor would be unable to perform all the necessary in-line 
operations at an adequate speed. 


'This explains why in most present industrial robots only two control modes are generally 
applied which correspond to the use of two types of models: 


e Position control which consists in assuming that the robot passes through a sequence of 
predetermined states £1, £2 ...£n corresponding to configurations 01,05...0,,. Position 
control makes thus use of the geometric kinematic model describing equation (3.1.1) and 
implies inverting it for all specified states; 


e Velocity control makes use of a differential kinematic model which results from a lineariza- 
tion of the kinematic model. The differential model relates displacement increments Ax 
in operational space to joint displacement increments in A0 configuration space. 


Figure 3.1.4 illustrates the principles of position control and velocity control and shows the 
difference between them. 


3.1.5 Level 3 of control or servo-system level 


'This concerns the standard practive of robotics. The scope of this last level of control is variable 
however, according to the control mode adopted. 


e When the control is based on the use of a dynamic model, what is controlled at this level 
is one of the bi-directional equations. 


V(t) ^ F(t) (3.1.4) 
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Figure 3.2.1: Planar manipulator with three links 


Vit) e F(t) e C(t) (3.1.5) 
Let us note that the current tendency in research is to design motors with integral stepdown 


gears, in which case it is the relationship (3.1.4) which is controlled directly. 


e In the case of control based on a kinematic model, the only way to control the dynamics 
of the mechanical structure is through the servo-system. 


3.2 Kinematic model of a robot manipulator 


In order to illustrate the concept of kinematic model let us consider the planar robot structure 
of figure 3.2.1. It has three hinge joints with rotation axis along z directions. Let 01, 05, and 63 
be the corresponding angular displacements. 


The operational space is defined here by the position (x,y) of the end point of the effector 
and its orientation. It can be expressed by three equations. 


x= lisin04 +l, sin(01 + 05) + Is sin(01 + 02 + 03) 
y = lı cos + ly cos(01 + 02) + l3 cos(01 + 02 + 03) +1 
p= 0i +02 t 63 (3.2.1) 


or, in matrix form 

x = f(0) (3.2.2) 
where z — [x y 0 ld represents the operational space, and 0 = [61 05 05 ie defines the configura- 
tion space. The system of equations (3.2.1) is the geometric kinematic model of the manipulator. 


Its inversion is straight forward when calculating first 09 and 01, what can be made by 
evaluating the position of point P’ = (z', y) at the wrist hinge. From the coordinates of P’ 


z'— x — lasinó 
y = y — lacoso — l (3.2.3) 
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a’ = ly sin 04 + lo sin(61 + 02) 
y = li cos0, +12 cos(04 + 03) (3.2.4) 


Taking the sum of these two equations, one deduces 
a^ py? = I2 + I2 + lily [cos0; cos(0, +02) + sin; sin(£ + 65) ] 
Two solutions for 05: 


0 = 


12 12 2 2 
-ly - l 
E cog"! (EE (3.2.5) 


To identify 61, let us note that 


z = ly sin 04 e l5 sin(61 + 02) (3 2 6) 
y licos04 + lzcos(01 + 02) E 
which can be rewritten 
x ly tan 04 + lə tan 04 cos 05 + lə sin 05 (3.2.7) 
y li + [2cos05 — lətan 0, sin 05 n 
and solved with respect to 01. One can see at first that: 
“(ly 4- la sin63) — lz sint 
tan 04 = 8t 5 7 " 
ly + l5 sin 05 + l2 sin 05 
The solution can be simplified if one notices that tanı = tan(o — 8) with tana = z'/y' and 
tan 3 = Lanes. It comes: 
i lg sin 05 
Op ed n E a M 3.2.8 
3 a (=) an lı + ly cos 02 ( ) 
Finally the degree of freedom describing the orientation of the effector is calculated by 
05 = $ó— (01 + 05) (3.2.9) 


Analyzing the results (3.2.3-3.2.9) shows that the inverse kinematic model is characterized 
by the following properties: 


1. It is described by a system of highly nonlinear equations. Its closed form solution has 
implied a specific step which consisted in decoupling the parameter describing effector 
orientation. Its solution could have also be obtained numerically but would have raised 
the problem of robustness for the algorithms used. 


2. A solution exists only as far as the point P’ = (z', y') is located inside the workspace defined 
at the level of the wrist hinge; equations (3.2.3) and (3.2.5) shows that the necessary 
condition is here 


(lı — l2)? < (x—lasin$)? + (y-l—lacosd)? < (l +l2)? (3.2.10) 


and depends on effector orientation. 
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Figure 3.2.2: Upper-and lower-arm configurations of the 3 DOF manipulator 


The dexterous workspace, which can be described as the part of the workspace which is 
reachable with arbitrary effector orientation, is further restrained by both conditions 


(Vz? --(y-1? + Is)? < (h + by 


(Vz? + (y-1)? e d^ < (L o1 (3.2.11) 


This can be show by looking for the extrema of 
g(ó) = (z—lasind)? + (y—1— lz cos ġ)? 


3. Inside the workspace, two solutions exist according to the sign chosen for solution equa- 
tion (3.2.5): they correspond respectively to the upper and lower-arm configurations 
(Fig. 3.2.2). 


4. When l4 = l2, a degeneracy occurs at point 
xz = lIssing y = l+lscos¢ 


Since the function tan-!(£) becomes then infinite: 05 is the equal to 180? and 0?, has an 
undetermined value. This corresponds to the situation where location points of joints 1 
and 3 come into coincidence. 


5. The kinematic model should also take into account the mobility restraints which would 
occur in an actual design. 


The differential kinematic model is obtained in the general case by differentiating (3.1.1) 
with respect to time in order to obtain velocities in configuration space. For a given component 
az; one obtains 


c Of; 
d; = »» 7 Å; (3.2.12) 
j=l 
or, in matrix form 
z= J0 (3.2.13) 
where the jacobian matrix of the system is defined by 


r= w= (f 
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In the particular case of three link manipulator defined in (3.2.1), equation (3.2.12) can be 
given in the explicit form 


t Cy + l2C12 + l3C123 loCi2 + l3C123 l3C123 Ó, 
y = —1,S1 — [3915 — l3 S123 —l25S12 — l3S123 —l3 S123 05 (3.2.14) 
0 1 1 1 fs 


with the definitions 
Sig... = sin(0; + 0; +...) 
Cij.. = cos(0i +6; +...) 


Its inversion is also easy to perform in closed form, provided that the velocities at the wrist 
hinge are extracted from (3.2.14) 


a i — la cosod 
P! ; 24 
H peus (3.2.15) 
under the form | 
à -— Cy F l2C12 l2C12 01 
| il | E | 21S) = Sis? bS | | by | (3.2.16) 


The inverse form to (3.2.16) is 


à] | 1 12515 l2C12 a! 
| 6. | ^ lhlaS5 | LS) — Sig —h C3 — l2C12 y (5211) 


Let us next replace i/ and y’ by the expressions (3.2.15). The inverse relationships take then 
the form 


: 1 s 3 1 
0; = — (la S12 $ + la Cia d + lala $3 ¢) 

lila S2 
1 , ; , 
05 — Ls; ((ly S3 + 1.512) t + (li Cr + l2 Cia) 9 + (lila S23 + l5 l3 $3) Ø) 
63 — $ = 6, = 65 


It is straight forward to deduce from them the inverse jacobian matrix 


1 l3 S12 lo C12 lə l3 Ss 
Jg. z TLS —(lh $4 + lo $13) —(l Cy + la C423) —(l l3 S23 + Ig Is $3) (3.2.18) 
apis lı Cy lı Cy lı l2 S2 + lı l3 S23 


It is need for control purpose, the kinematic model can also provide accelerations at joints in 
terms of accelerations in the operational space and joint velocities. Indeed a second derivation 
of equation (3.2.12) yields to the relationschip 


m * D an i 2 $ . H 
# = eh 6+ 3 a (3.2.19) 
j j 


which can be rewritten in matrix form with the following definitions: 
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Figure 3.3.1: Trajectory description of the manipulator 


- The matrix of squared velocities 
ġ = [ë (0, 62) 62... il (3.2.20) 


- The matrix of associated coefficients 


9 fi 
A= 3.2.21 
Lo A. ( ) 
The resulting expression 
à = J! E = A (3.2.22) 


shows that the accelerations at joints are also deduced in terms of the inverse jacobian matrix. In 
the case of the three-link manipulator, the matrices 0? and A have respectively the dimensions 
6 and 3 x 6. 


3.3 Trajectory planning 


Suppose that the two-link manipulator is in the configuration shown as Po in figure 3.3.1; and 
suppose that it is required that it be moved to the destination configuration shown as P. 


Trajectory planning converts a description of the desired motion 
« MOVE (P1) »; 


into a trajectory defining the time sequence of intermediate configurations of the arm between 
the origin Po and the destination Pj. The output of trajectory planning is a sequence 


(0X) k= leron 


of configurations of the arm. The configurations, possibly together with their first and second 
time derivatives, are then shipped off in succession to the servo-mechanisms controlling the 
actuators that actually move the arm. 
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(a) (b) 


Figure 3.3.2: Comparison of trajectory linear interpolations in joint space and in cartesian space 


'The actual trajectory between points Pj and P, depends on the point of view adopted to 
describe it. T'wo descriptions of configurations can be adopted, namely configuration (or joint) 
space description and operational (cartesian) space description. 


'To illustrate the difference between both approaches let us consider the simple problem of 
generating a linear trajectory from Po to P, with the two-link manipulator of figure 3.3.1. 


3.3.1 Joint space description 
A linear interpolation between origin and destination provides the trajectory description 
e(t) = (1—t)0(0) + t0(1) t € [0,1] (3.3.1) 


Here, the £ parameter describes simply the distance along the curve. It is thus normalized so 
that t = 0 at the origin configuration and t = 1 at the destination. 


Figure 3.3.2(a) compares the resulting trajectory to the desired one and figure 3.3.2(b) 
displays a few intermediary configurations of the two-link arm. 


It shows that interpolating in joint space is acceptable only as far as the path followed 
between Pp and P, is completely free. This corresponds to point-to-point motion control mode 
which characterizes many of the industrial robots available today. 


3.3.2 Cartesian space description 


In the operational space, the linear interpolation 
a(t) = (1—1)z(0) + taz(1) t € [0,1] (3.3.2) 


provides directly the desired trajectory. It can be achieved either through velocity control mode 
based on the kinematic differential model (3.2.13), in which case parameter t has meaning of 
the time, or using the concept of bounded deviation path. 


'The latter approach uses the fact the linear interpolation in joint space is very efficient to 
implement. It does not achieve straight line motion in cartesian space, but may depart from it 
by an acceptably small amount if the source and destination points correspond to nearby points 
in space. An efficient method for obtaining a bounded deviation path consists to introduce 
control mid points, or knot points, in a recursive manner. 


'The concept is illustrated by figure 3.3.3 which shows how a straight line trajectory can 
be refined progressively up to obtaining the result (c). The well known Taylor's algorithm for 
generation of trajectories is based on it. 
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Figure 3.3.3: Generation of straight line motion in a recursive manner 


- (a) First recursive call to the knot point generation algorithm 
- (b) Intermediate configurations of case (a) 

- (c) Knot points generated by a second recursive call 

- (d) Knot points generated by a third recursive call 

- (e) Intermediate configurations of case (d) 

- (f) 


f) Comparison of the successive trajectories 


3.3.3 Description of operational motion 


'The concept of cartesian path that we have just described does not necessarily include the 
concept of time course along it. Taking the time as a path parameter allows also for a control 
of velocity and acceleration along the trajectory: it is thus customary to distinguish between a 
path and a trajectory by defining the trajectory as the time course along the path. 


Achieving velocity and acceleration control of the robot arm along the trajectory may be 
important for several reasons: 


e Certain types of applications imply real time control of the position / orientation of the 
effector: arc welding, picking objects on a moving conveyor, etc. 


e Trajectory specifications have to be compatible with the available acceleration power of 
the various joints; 
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e Time optimization of trajectories will become a subject of increasing importance. 


Describing operational motion implies controlling the speed and the acceleration of the ef- 
fector along its trajectory. In a particular, mechanical shocks have to be avoided by achieving 
continuous velocity and acceleration transition between consecutive trajectory segments. Ap- 
propriate time interpolation of trajectory segments is thus an important aspect of trajectory 
generation. 


3.4 Dynamic model of a robot manipulator 


3.4.1 The concept of dynamic model and its role 


'The dynamic model of the robot manipulator expresses the relationship between the various 
forces acting on the mechanical structure and the resulting displacements, velocities and accel- 
erations. The forces implied in robot motion may have several origins: 


- the torques delivered by the motors, 

- the inertia of members, 

- the gravity, 

- the loss of energy (damping, friction ...), 


- the interaction with the undergoing task. 


Just as in the kinematic analysis, one may distinguish between the direct dynamic model 
and the inverse dynamic model. 


Given an initial state of the mechanical structure (i.e. displacements and velocities at joints 
at time t = 0) and the time history of torques c(t) acting at joints, the direct dynamic model 
allows to predict the resulting motion 0(t). When associated with the direct kinematic model, 
it yields to a prediction of the trajectory x(t). 


'The mechanical form of the dynamic model is a nonlinear system of second order differential 
equations to be integrated in time. 


Its primary purpose is to obtain a computer simulation of robot dynamic behavior. Asso- 
ciated with a model of servo-systems,it can also provide an evaluation of the characteristics of 
the overall response of the manipulator and its control system (Figure 3.4.1). 


The direct dynamic model can also be used for control purpose (for example, in a control 
mode based on a reference model) in which case a simplified version of it is generally sufficient. 
Despite the simplifications that are brought to the model, the main limitation stems from the 
need to integrate the model in real time. Therefore, the use of a direct dynamic model aiming 
at the synthesis of an appropriate control law is still largely a research subject. 


The inverse dynamic model allows to predict the torques needed to reach or maintain a 
specified geometric configuration. It implies a computer effort which is of an order of magnitude 
less than the direct model plays therefore a more immediate role in robot design and control. it 
can be used for two purposes: 


e At the design level, in order to evaluate the required mechanical characteristics of actuators 
and transmission devices and to predict the dynamic behavior of the system; 
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Figure 3.4.1: Computer simulation of robot dynamic behavior using a direct dynamic model 


e At the control level, to predict the torques delivered by the motors in view of the synthesis 
of a dynamic control law. 


The principle of a dynamic control law which makes use of an inverse dynamic model is 
outlined in Fig. 3.4.2. Its main advantage by comparison with classical control lies in its ability 
to provide adaptation to changes of inertia resulting from variable geometric configuration. 


A dynamic model of either direct or inverse type can be constructed according to two tech- 
niques which have their respective advantages and drawbacks. 


e The Euler-Newton formalism proceeds by splitting the system into individual components 
and writing vector relationships which express dynamic equilibrium of the individual parts. 
It is well suited to a recursive computational procedure which leads to a minimum of 
arithmetic operations. 


e The Lagrangian approach, which is based on the well known Lagrange equations of the 
classical mechanics, presents mainly the advantage of being systematic and of simple 
application; it is based on the evaluation of energy quantities such as kinetic energy due to 
gravity and virtual work associated to applied torques and externalloads. It may lead also 
to recursive computation procedures, but in a not so evident manner than Euler-Newton's 
formalism. 


The following introduction to the concept of dynamic model will be based on the Langrangian 
approach. 


3.4.2 Dynamic model of a two-link manipulator 


Let us consider the 2 DOF manipulator of fig. 3.4.3. The masses mı and mz of members are 
supposed concentrated at joints in order to simplify the model to a maximum. 


The Langragian formalism of classical mechanics stipulates that the kinetic energy U of a 
system with n degrees of freedom q;, (i = 1,...,n) can be expressed in the forms 


T = T(q,d.t) and U= U(q,t) 


16 
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Figure 3.4.2: The principle of dynamic control 


Figure 3.4.3: Model of a two-link manipulator 
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The dynamic behavior of the system is then governed by the dynamic equilibrium equations 
written in Lagrange's form 


Q;(t) = (3.4.1) 


d(OTN OT if ou 
dt Odi Oqi Odi 

where the Q; denote the generalized external forces conjugated to the generalized displace- 
ments qi. 


Let us decompose the kinetic and potential energies of the system in a sum of member 
contributions 
7T — +h and U = U + U2 (3.4.2) 


The kinetic energies of the masses m, and mə are simply given by 


1 1 
T= gni and do = g2 (3.4.3) 


where v; and v2 are the absolute velocities of the corresponding points. They are calculated in 
terms of the generalized degrees of freedom 0, and 05 by 


eat a 
= 176; 

vj = dj + ý 
= 126? + 12(6; + 62)? + 2l;lo(0, + 05) cos 0261 (3.4.4) 


The potential energies of the masses m and m» under the action of gravity are similar given by 


Ui = mig 
= migli(1— cos 81) 
Uz = m2gy2 
= magli(l— cos041) + magl2(1 — cos(0 + 02)) (3.4.5) 


The application of the Lagrange equations (3.4.1) provides the algebraic expressions for the 
generalized forces conjugated to joint displacements 01 and 62 


c1(t) = mii as ma205 T b110? T b12202 x b1120102 Tt gi 


c2(t) m1261 + ma3605 + 531102 + b22202 + 05210105 + ga (3.4.6) 


II 


The meaning of the different inertia coefficients of the model is the following 


e the m;; are the principal inertia coefficients. Their explicit form is 


mıı = mil? + m(l? + i5 + 2lil5 cos 05) 


mals 


I 


M22 


e the symmetrical term 71? and m»; are the inertia coupling terms 


Mı? = M21 = mal2 + Molila cos 05 
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e the coefficients give rise to centrifugal and Coriolis forces 


b111 = 0 b122 = — Məlılə sin 05 
b211 = Mayle sin 02 b222 = 0 
b112 = — 2malyle sin 05 b221 = 0 


It can be observed that most of these coefficients are strongly dependent on the instantaneous 
configuration. 


e The terms gi and gz denote the torques produced by the gravity. One finds explicitly 


gi = (mı + mə)glı sin 0 + maglz sin(01 + 02) 
ga = maglz sin(01 + 02) 


3.5 Dynamic model in the general case 


In the general case, the dynamic model of a n-degree of freedom with open chain structure takes 
a form analogous to (3.4.6) 
MÖ + B@+g=c (3.5.1) 


where 
e M is the inertia matrix with dimension n x n 


e B is the matrix of centrifugal and Coriolis coefficients. It has dimension m x m with 
m — n(n + 1)/2 


e g is the matrix of gravity terms, with dimension n 


e c(t) is the matrix of applied torques, also with dimension n. 


All the coefficients of the matrices M, B and g are obviously very complex functions of the 
instantaneous configuration 6. 


3.6 Dynamic control according to linear control theory 


Our objective is to discuss how to cause a manipulator to actually perform a desired motion. 


If one excepts the case of manipulators driven by either stepper or pneumatic motors which 
can be controlled in an open loop fashion, one may consider that manipulators are powered by 
actuators which provide a torque or a force at each joint. Some kind of control system is then 
needed to generate appropriate actuator commands which will realize the prescribed motion. 


A manipulator can be regarded as a mechanism with an actuator at each joint to apply a 
torque between two neighboring links, and instrumented with position (and possibly velocity) 
sensors to measure the joint angular displacements and velocities. 


In order to cause the desired motion of each joint to be followed by the manipulators, we 
must specify a control algorithm which sends torque commands to actuators. Almost always 
these torques are compared using feedback from the joint sensors. 
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Figure 3.6.1: A typical control system 


'The control problem for manipulators is inherently non-linear. This means that much of 
the linear control theory described in this section for the synthesis of control algorithms is not 
directly applicable. 


Nevertheless, in most of the proposed solutions to the non-linear problems, one ends up 
using methods from linear control theory. This material will be quickly reviewed in the section 
6 and applied to multi-variable systems in section 7. The content of the these two sections is 
essentially based on ref. 5. 


3.6.1 The objective of control theory 


'The objective control theory is to analyze and synthetize like the one illustrated in figure 3.6.1. 
A typical system consists of 


e an actuator which is capable of changing the environment in some way. For the case of a 
robot joint, the actuator will be the electrical or hydraulic motor driving the joint; 


e a sensor of some kind which can measure some aspects of the actuator's effect on the 
environment. In our case, the sensor(s) will measure position and/ or velocity. 


Feedback is used as follows. The actual value measured by the sensor is compared with (i.e 
subtracted from) the desired value to form an error value. This error value is converted to an 
actuator control signal through multiplication with a positive gain. In this way, the actuator is 
commanded such that it always tends to reduce the current value of the error. 


3.6.2 Open-loop equations for motion of a physical system 


Let us consider the control of a very simple mechanical system such as the system figure 3.6.2. 
The control variable, x, is the position of the mass. The actuator (not shown) is capable of a 
force f to drive the mass which experiences also a spring force —k x and a viscous damping force 
—c t. The constants k and c are respectively the spring (stiffness) and damping constants. 


'The equation describing the motion of the system itself is obtained by summing all the forces 
acting on the body 
mi + ct+kz = f (3.6.1) 


It is the open-loop equation of motion. Because the equation (3.6.1) is of second order in time, 
we refer to the mechanical system of figure 6.2 as a second order system. 
3.6.3 Closed-loop equation of motion 


Let us next assume that we have position and velocity sensors which measure x and « for our 
mechanical system. We could then use this information to compute a value of f to apply to the 
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Figure 3.6.2: Mass-spring-dash pot system 
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Figure 3.6.3: Block diagram of a position regulating controller 


system through the actuator. For example, we could compute the control law 
f = — kpr — kyt (3.6.2) 


In order to analyze how the system would behave when controlled with this law, we write the 
closed-loop equation of motion by combining (3.6.1) and (3.6.2). This leads to 


mi + (c+ ku) + (k+ kp)z = 0 (3.6.3) 


By comparison with the open-loop equation of motion, the closed loop equation shows us that 
the system now acts as though the spring has the stiffness value (k + kp) and as though the 
damping has the value (c + k,). Hence we have used the control effort to change the apparent 
stiffness and damping of the original system. 


'This is perhaps the most basic application of control theory to alter the characteristics of 
the system under control in order that the system behaves in some desired way. 


To build a position-controlling system we would choose a large value for kp so that the mass 
would act as if an extremely stiff spring was holding it in position. Such a control system would 
try to maintain the nominal position of the mass despite various disturbance forces. This type 
of control system is called a regulator because it attempts to regulate the value of the control 
variable to some constant value. 

Figure 3.6.3 shows a block of diagram of the control system specified by the control law 
(3.6.3). The mechanical system is shown only as a black box whose input is the force which we 
can command with the actuator,and with outputs x and « which can be read with the sensors. 
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3.6.4 Stability, damping and natural frequency of the closed-loop sys- 
tem 


Given a control law such as (3.6.2) and the resulting closed loop equation such as (3.6.4), the 
performance of a system is usually discussed in terms of two central features of the control 
system, namely 


e its ability to respond to changes by the desired output value; 


e its ability to suppress disturbances. 


As far as performance is concerned, the first requirement of a control system is its stability. 
A possible definition of stability is the following: 


a system is stable if, given an input function (or disturbance function), the output 
remains bounded. 


In order to determine stability, along with other performance measures, we must solve the 
closed-loop equation. For example, to solve the second-order differential equation with constant 
coefficients (3.6.3), let us assume a general solution of the form 


ms? +(c+ky)s+(k+kp) = 0 (3.6.4) 


It has two roots, sı and s2, which are given by 


k +t ky)? — Am(k + k 
$12 = ees vo viet ku)? — 4mlk + kp) (3.6.5) 


2m 2m 


and their nature depends upon the values of the gain k, and kp which are introduced in the 
system. The roots are complex 
s = —-axtif (3.6.6) 


provided that the gains are such that 
(c+ ky)? < 4m(k + kp) (3.6.7) 
in which case the solution to (3.6.3) may be conveniently written in the form 
r = ae” sin(wn V1 — r2t + ¢) (3.6.8) 
where 


c E, 
2m 


is the time decay coefficient, which remains positive as long as c+ ky > 0; 
Vk k 
wn = ~—* (3.6.9) 
m 


is the natural frequency of the system. It remains real as long the effective stiffness k + kp is 
positive, in which case it is related to the magnitude of the roots of the system by 


Wn = Va? + f? (3.6.10) 
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Figure 3.6.4: Response of the 1 DOF system as function of pole location 


is the damping ratio of the system, and is calculated by 


ky 
s GT ee (3.6.11) 


24/m(k + kp) Va? + p? 
Different behaviours can be observed depending upon the relative damping of the system. 
e If T = 0, the system has no damping and it will never stop moving once disturbed 


e If 0 < T < 1, the system is lightly damped and it returns to its equilibrium position 
according to an oscillatory motion given by equation.(3.6.8). 


e If 7 > 1, the system is over-damped and it does not oscillate at all. The motion is of 
exponential type 
zc = Ae?! + Be? 


with roots calculated by (3.6.5). 


e If 7 = 1, the system is critically damped and possesses two equal and real roots 


$12 = — Q 


E 


Critical damping allows the fastest return to the nominal position without being oscillary. 


Figure 3.6.4 shows the behavior of the system according to the location of the roots of the 
characteristic equation on the real-imaginary plane. 
It indicates qualitatively what the shape ,of the response to imposed initial conditions would 
be. Notice that if the real part of the roof is positive, the response is unstable. If the roots 
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are complex, the response is oscillatory and if they are real, the response is the sum of two 
exponentials. For simplicity only one of the roots is displayed in the figure, the second one 
being its complex conjugate. 


3.6.5 Position control 


Our control system developed so far has not input; it is simply a regulating system. 


Consider again the problem of controlling the system of figure 3.6.2. To add a desired 
position input, let us use the control law 


f = kp(aa— x) — kyt 
where xq is the target position of the mass. The resulting closed-loop equation is 
më + (c+ ky) + (k + kp)x = kyxa (3.6.12) 


If the target position zq is constant with time, position error 


€ = 3iq—cx 
is such that è = — t and ë = -— à. It is thus governed by a similar equation 
(k--ky)e + (c+ k,))é + mé = kag (3.6.13) 


Assuming that rg is imposed in a step fashion, the system will asymptotically move its equilib- 
rium position to the steady-state response of (3.6.13) 


kp 
= .6.14 
x DES Ld (3.6.14) 
to which corresponds a steady-state error 
k : 
z= 7 5 Tq such that QE e —0 (3.6.15) 
3.6.6 Integral correction 
A modified control law of type 
t 
Pe Renae & dde ky | adh de (3.6.16) 
0 


includes proportional integral and derivative correction and is called PID controlled law. It will 
guarantee that any steady state error would cause the integral term to build up until it generates 
a sufficient force to cause the system to move such as to reduce the steady state error. Time 
derivation of the corresponding closed-loop equation yields to 


me + (c+k,)é + (k+ kpjė + kre = 0 (3.6.17) 


The performed of the PID controller is thus governed by a third order system. The roots of its 
characteristic equation may be adjusted so as to match those of the equation 


(s? + 2Twnstw2)(st+y) = 0 (3.6.18) 
where y is spurious root. The gain ky, kp and ky are then related to 7 and y by 
ky = w2ym 
ky = m(2rTw4 +7) —c (3.6.19) 


kp = (w2 +2r7)m — k 
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Figure 3.6.5: Control system with complete trajectory input 


3.6.7 Trajectory following 


Trajectory means, as previously stated, the time history of the position variable and the corre- 
sponding velocities and accelerations. 

While the control law (3.6.12) was sufficient for a system commanded with step type inputs, for 
a general position trajectory it is useful to build a control system which has 3 inputs: desired 
position, velocity and acceleration. If these inputs are available from the trajectory planning, a 
reasonable choice of the control laws is 


f = mia + ky(ta— t) + kp(ta — m) (3.6.20) 


to which an integral correction may eventually be added. 

To make this choice clear, let us consider the extremely simple case of a unit mass with 
no elastic restoring force, no damping and driven by the control law (3.6.20). The open-loop 
equation is simply 

i=f (3.6.21) 


combining (3.6.20) with the control law (3.6.22), we have 
B= fat kyé+ kpe (3.6.22) 


which may then be written as 
ë+ kyè + kpe = 0 (3.6.23) 


This form is particularly nice because the error dynamics is chosen directly by gain selection. 
A good choice is to choose gains for critical damping, in which case errors are suppressed 
in a critically damped fashion. Figure 3.6.5 shows the general structure of such a trajectory 
following controller. Many times, an integral correction is added to the servo law as described 
in the previous section. 


3.6.8 Control law partitioning 


In preparation for designing control laws for more complicated mechanical systems, let us con- 
sider again the mass-spring-damper system of figure 6.2 and construct its control law within a 
certain structure which will be helpful in understanding the control of more complicated systems. 
We wish decompose the controller of the system into two parts: 
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e The first part of the control law is model-based in that it makes use of the parameters 
of the particular system under control. It is set up such it reduces the system so that it 
appears to be a unit mass; 


e The second part of the control law is error driven in that it forms error signals by differen- 
ciating desired and actual variables and multiplies these error by gains. The error driven 
part of the control law is also the servo portion. 


Since the model based portion of the control law has the effect of making the system to 
be a unit mass, the design of the servo portion is very simple. Gains are chosen as if we were 
controlling a unit mass system. 


The partitioned control law is thus assumed of the form 
f=of +8 (3.6.24) 


where o and f are functions or constants chosen so that when f' is the new input of the system, 
the system appears to be a unit mass. With this structure of control the system equation is 


më + cé+kae = of 4 (3.6.25) 


Clearly, in order to make the system appear as a unit mass f ' input we should choose o and 3 
as 


a =m 


B= ct + kc (3.6.26) 
in which case the system equation becomes 
ï= f (3.6.27) 


The servo portion of the control law is computed next in order to answer to the objective of 
trajectory following. From equation (3.6.20) we obtain 


f = ža + ké + kpe (3.6.28) 


In this case, kp and k, are computed for a system given by equation (3.6.27) and are particularly 
easy to compute. 

Combining next equations (3.6.25) and (3.6.28) we can write the closed loop equation for 
the system. Making use of the model based part (3.6.25) reduces it to the equation governing 
the error of the system 


E+ kė + kpe = 0 (3.6.29) 


Its frequency behavior is governed by the parameters 


k 
Wn = Vh > (3.6.30) 
p 


and it is critically damped when k? = 4k,. Figure 6.6 shows the general form of the partitioned 
servo law with trajectory following inputs. 
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Figure 3.6.6: General form of control law 


3.7 Motion Control of Non-Linear and Time-Varying Sys- 
tems 


In the preceding section the mathematical form of the control law arose from the fact that 
the system under consideration could be modeled as a linear constant coefficient differential 
equation. For systems whose parameters vary in time or systems which are by nature non 
linear, efficient solutions are however more difficult to obtain. 


In a very broad sense, mainly two approaches may be followed to control systems involving 
non-linearities: 


e Linearization may be used, when non-linearities are not too severe, to derive linear models 
which are approximations of the nonlinear equations in the neighborhood of an operating 
point; 


e Adaptation of the control law consists to change with time the coefficients of the control 
law according to variations of systems parameters. 


Linearization does not lead to an efficient solution for controlling robot manipulators since 
they have been shown to have a highly motion dependent dynamics. We will thus concentrate in 
what follows on an adaptive technique in which a nonlinear control law is applied to the system 
in such way that it performs linearly. 


3.7.1 Design of nonlinear control laws 


Various adaptative techniques can be proposed, but the best suited to robot motion control con- 
sists probably to construct a linearizing control law as suggested by the control law partitioning 
method described in the former section. The model based part of the control law is then motion 
dependent and calculated such that the servo-error of the system performs in the same manner 
as if the system was linear. Let us consider the case of controlling the more general one-DOF 
system 

miá-Jg(r,i) = f (3.7.1) 


where the function g(x, t) presents general motion-dependent forces such as 


- Elastic and inelastic (plastic, visco-plastic ...) restoring forces; 
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Figure 3.7.1: Nonlinear control system with error performance of linear system 


- Linear or nonlinear (dry friction ...) damping forces, 
- Gyroscopic forces. 


The model based portion of the control law is f = af "+ B where 


a=m 
B = g(x, i) (3.7.2) 

and the servo portion is as always 
f = ra + kyétkpe (3.7.3) 


where the values of the gains are calculated from some desired performance specification. Fig- 
ure 3.7.1 shows a block diagram of this control system. 

While the field of nonlinear control theory is quite difficult in general, we see that in specific 
cases as covered by equation (3.7.1) it is non prohibitively difficult to design a nonlinear control. 
It can be summarized as follows: 


Compute a non-linear model based control law which ’cancels’ the non-linearities 
of the system to be controlled. The system is then reduced to a linear system 
which can be controlled using a simple linear servo law. 


'The linearizing control law can be constructed provided that an inverse dynamic model 
of the system is available. The non-linearies in the system cancels with those of the inverse 
model, leaving a linear closed loop system. Obviously, to do this cancelling, we must know the 
parameters and the structure of the non-linear system. This may be the most difficult part 
when dealing with real nonlinear systems. 


3.8 Multi-Variable Control Systems 


Controlling a robot manipulator is a multi-input/ multi-output problem. A desired trajectory 
of the effector can be translated in terms of desired joint positions, velocities and accelerations. 
'The control law must compute a vector of joint actuator signals from the knowledge of the 
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actual configuration and the desired trajectory. Our basic scheme of partitioning the control 


into a model-based part and a servo part is still applicable, but will appear in the matrix form. 


The control law is now the form 
f-Af +8 (3.8.1) 
where, for a n-degree of freedom system, 
e f, f are n-dimensional vectors 


e A is a nzn matrix. 

Note that the matrix A is not necessarily diagonal, but is rather chosen to decouple the 
n equations of motion. If A and f are correctly chosen, then from the f input the system 
appears to be n independent unit masses. For this reason, in the multi-dimensional case the 


model-based portion of the control law is called the linearizing and decoupling law. The servo 
law for a multi-dimensional system becomes 


f = d, + Kè + K,e (3.8.2) 


where K, and K, are now nxn gain matrices. They are generally chosen to be diagonal with 
constant gains on the diagonal. 


3.9 Multi-variable Problem Manipulators 
In the case of manipulator control, we have shown that the equations of motion take the form 
C(t) = M(0) + B(0) 8? - g(0) (3.9.1) 


where M is the inertia matrix, B the matrix of Coriolis and centrifugal coefficients and g is the 
vector of gravity coefficients. All their elements are complex functions of 0, the n-dimensional 
vector of joint variables. Additionally, we may incorporate a model of friction or other non-rigid 
body effects in the form 


C(t) = M(6)6 + B(6) 6? - g(0) + f(0,0) (3.9.2) 


The problem of controlling a system like (3.9.2) can be handled by the partitioned controller 
scheme that we have introduced throughout this chapter. In this case we have 


c(t) = Ac + B (3.9.3) 
and we choose 
A-—M 
B = B(0)0?  g(0) + f(0,0) (3.9.4) 
with servo law ; 
c = 04 + K,ét Ke (3.9.5) 


where the servo error is calculated by 


e = 04-0 (3.9.6) 


3.9. MULTI-VARIABLE PROBLEM MANIPULATORS 29 


ee ' 0 
Oa ES | M(0) .—H(—*.4 MANIPULATOR |, 
ane 0 
K, IK, B(0)6? +8 (0) + £ (0,9) 
K^ 
ok de EN 
Qa Ru i : 
pow | 
Oa : 
mi 


Qu teo 9 , DYNAMIC | +35) € J MANIPULATOR 


os MODEL ud ó 
K, K, 
^ 
e | é 
9 a > (>) * 
t NES 
04 = 
7O 


Figure 3.9.2: Manipulator control system with external disturbance included 


The resulting control scheme is represented by the figure 3.9.1. The entire inverse dynamic 
model appears in one box to indicate that the decoupling and linearizing procedure remains the 
same regardless of how the inverse dynamic model is computed 

If we have good knowledge of all the parameters of the manipulator then the control scheme of 
figure 3.9.1 will provide a performance of the system with the chosen gains. 


However, the assumptions that we have made to develop controllers for robot manipulator 
are rarely available for decoupling and linearizing the control algorithm as described. 


One way to analyze the effect of error in the model of the system is to indicate a vector of 
disturbance torques acting the joints. In figure 3.9.2 we have indicated these disturbances as an 
input: they arise from any un-modelled effect in the dynamic equations, and they include also 
the effects of resonance and ”noise”. Writing the system error equation with the inclusion of 
unknown disturbances 


ë + K,é + K,e = M"(6)nq (3.9.7) 


where na is the vector of disturbance torques at joints. The left-hand side of (3.9.7) is uncoupled, 
but from the right-hand side we see that a disturbance of any particular joint will introduce 
errors at all other joints since M is not diagonal. 


30 CHAPTER 3. BASIC PRINCIPLES OF ROBOT MOTION CONTROL 


3.10 Practical Considerations 


The assumptions that we have made to develop controllers for robot manipulators are rarely 
true in practice. The exact models are rarely available for decoupling and linearizing the control 
algorithm as described. It is thus important to discuss the practical problems faced by the 
control engineer in the design of control systems and describe the solutions which are adopted 
industrially to overcome them. 


3.10.1 Lack of knowledge of parameters 
Even if a model is available, the parameters of the model are often not known accurately: 


- Friction effects are extremely difficult to predict with sufficient accuracy; 


- The mass properties of the system and the interaction forces with the external world are 
changing during the task, so that the maintenance of an accurate dynamic model is gen- 
erally impossible. 


Since the model of equation (3.9.2) will never be perfect, let us distinguish between the model of 
the system and its actual properties. Let us note by an asterisk our model of the manipulator. 
Then, perfect knowledge of the model would mean 


M*(0) = M(0) 
B*(0,0) = B(0,0) 

g'(0) = g(0) (3.10.1) 
f'(6,0) = f(0,0) 


so that, although the manipulator dynamics is given by equation (3.9.1), our control law com- 
putes with 


c= Ac + B 
A = M* (3.10.2) 
B = B*(0,0) + g'(0) + f'(0,0) 


so that decoupling and linearization are not accomplished perfectly. By writing the closed loop 
equation one observes that the lack of knowledge of parameters is responsible for a noise input 


na = M*^" [(M - M*)8 + (B- B')? + (g-g") + (f-£17)] (3.10.3) 
which influences the response of the servo equation 


ë+ K,é€+ Kpe = na (3.10.4) 


3.11 Time Effects in Computing the Model 


In all our considerations of control laws based on system modeling we have made the assumption 
that the entire system was running in continuous time and that the computations in the control 
laws are made with infinite speed. In practice, it is necessary to take account of time effects 
such as 


- 'The sampling rate at which sensor information is read and sent to the controller, 
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- The controller frequency giving the rate at which control signals are computed and sent to 
the actuators. 


To analyze the effects of time delay in computing the model, sampling rate and controller 
frequency we should use tools from the field of time control, but this is beyond the scope of this 
presentation. 


Let us simply mention that in order to have the confidence that computations are performed 
quickly enough and that the continuous time approximation is valid we have to take account of 
the following points: 


e Tracking the reference inputs: the frequency content of the desired or reference input 
places an absolute lower bound on the sampling rate. The sampling rate must be at least 
twice the bandwidth of reference inputs, but this is not usually a limiting factor. 


e Disturbance rejection: if the sampling period is longer than the correlation time of 
the disturbance effects ( assuming a statistical model for random disturbances) then these 
disturbances will not be suppressed. In practice, the sample period should be 10 times 
shorter than the correlation time of the noise. 


e Anti-aliasing: aliasing occurs when signals coming from analog sensors are converted to 
digital form. There will be a problem unless the sensor’s output is strictly and limited 
using an anti-aliasing filter. It is also possibly to verify in practical cases that the sampling 
rate is such that the amount of energy in the aliased signal remains small. 


e Structural resonance effects: a real mechanism having finite stiffness it will be subject 
to various kinds of vibrations. The influence of these vibrations on the controller has 
to be suppressed by choosing a sampling rate which is more than twice the fundamental 
vibration frequency. A rate 10 times higher is recommended. 


3.12 Present Industrial Robot Control Systems 


Because of the problems with having accurate knowledge of parameters, it is not clear whether 
it makes sense to consume time in computing a complicated model based control law for ma- 
nipulator control. T'herefore, the present day manipulators are still controlled with very simple 
control law which are generally of error driven type. It is still instructive to consider these 
simpler control schemes within the context of the partitioned controller structure. 


3.12.1 Individual joint PID control 


Most present industrial robots have a control scheme which could be described in our notation 
by 
A-—I and B=0 (3.12.1) 


where I is the nxn identity matrix. The servo portion is 
.. t 
c = u + Ke + Kpe + Ki] edr (3.12.2) 
0 


where Kz, K, and K, are constant diagonal gain matrices. In many cases, 64 is not available, 
and this term is then simply set equal to zero. That is, most simpler controllers do not use at 
all a model based component in their control law. 
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'The advantage of this PID control scheme is that each joint has a separate control system 
which can be implemented using a separate microprocessor. The servo-error equation is obtained 
(assuming here K; = 0) by writing 


M* =1 
ë + Kė + K,e = (M-I)Ó; - BP +g+f (3.12.4) 


Average gains have to be chosen to give approximate critical damping in the center of the robot 
workspace. In various extreme configurations of the arm the system becomes then either under- 
damped or over-damped. Maintaining them at a very high value is necessary to minimize the 
disturbance effect due to the absence of any model in the control law. 


3.12.2 Individual joint PID control with effective joint inertia 


Some robots have a control scheme in which the inertia properties of the system are modeled to 
some extent and introduced in the control law by taking 


A = M'(6) 
B=0O (3.12.5) 


where M’ is a n x n matrix with functions of configuration on the diagonal. This changing 
inertia is modeled to try to cancel partially the effective inertia of the system. 


Such a model will be easier to keep near critical damping, but will still have to continually 
suppress disturbances which result from coupling between joints. 


3.12.3 Inertial decoupling 


A further improvement consists to have a control system in which inertia effects are modeled 
properly 


A = M"*(0) 
8-0 (3.12.6) 


where M* is a n x n model of the inertia matrix. The closed loop error is then governed by 
ë + K,é+ K,e = M* (B + g 4 f) (3.12.7) 


The main error to be controlled is due to gravity and friction. Including integral control as 
suggested by equations (3.6.16) and (3.12.2) is thus essential. 


3.13 Cartesian Based Control Systems 


In this section we come back to the concept of cartesian based control which we have briefly 
discussed in the section 1. The concept of cartesian based control is important since accurate 
trajectory planning requires defining the trajectory into a cartesian space. Therefore, cartesian 
based control systems will certainly arrive at a fully industrial stage in a near future. 
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Figure 3.13.2: General structure of cartesian control scheme 


3.13.1 Comparison with joint based schemes 


So far we have assumed that the desired trajectory is available in terms of joint position, velocity 
and acceleration. We have then developed joint based control schemes in which we develop 
trajectory errors in term of desired and actual quantities in joint space. 


Very often trajectory planning requires the effector to follow straight lines or other curved 
paths defined in cartesian coordinates. 


It is then theoretically possible to control the trajectory as suggested by figure 3.13.1 by 
converting first trajectory information into joint space variables, and applying next any of the 
control laws developed in the previous sections. 

The trajectory conversion is a time consuming operation even if done analytically since it involves 
the three operations 


64 = INVKIN (z4) 


ĝa = J-!(za)óma (3.13.1) 
64 = Iita + Jd. 


'The solution of the kinematic problem has to be performed explicitly while calculating ve- 
locities and accelerations can be done numerically. Numerical differentiation is however a source 
of noise and lag in the system which have to be avoided. 


A alternative approach consists to control directly the cartesian error, in which case the 
general structure of the controller is that of figure 3.13.2. The output of the system, i.e. the 
sensed position of the manipulator, has to be converted by means of the kinematic relationships 
into a cartesian description of position. The current cartesian is then compared to the desired 
one in order to form errors in cartesian space. 


34 CHAPTER 3. BASIC PRINCIPLES OF ROBOT MOTION CONTROL 


IODE ES Gains | 6 , arm (P 


Direct Kinematic * 


Figure 3.13.3: Inverse Jacobian cartesian control 
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Figure 3.13.4: Transposed Jacobian cartesian control 


The most classical implementation of cartesian control and the most intuitive one also is the 
inverse Jacobian cartesian control. Its general structure is that of figure 3.13.2, but use is made 
of the differential kinematic model in a first step to convert cartesian errors 6x into joint space 
error 60. The resulting errors 00 are then multiplied by gains to compute torques which will 
tend to reduce these errors. Note that the figure 3.13.3 shows a simplified version of such a 
controller in the sense that, for clarity, velocity feedback has not been represented. It could be 
added in a straight forward manner. 

Another possible implementation of cartesian control could consist of converting the cartesian 
errors into forces first, and then transforming these forces into equivalent joint torques which 
tend to reduce the observed errors. The cartesian scheme so obtained is shown on figure 3.13.4. 
It presents the advantage that inverting the Jacobian matrix real time is avoided. 


'The forces f on figure 3.13.4 have the meaning of forces to be applied at the end effector 
level in order to reduce the cartesian error. 
'The exact dynamic performance of such cartesian controllers can only be predicted by numerical 
simulation. One observes that both can be made stable by appropriate gain selection, but their 
dynamic response varies with arm configuration. 
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Chapter 4 


KINEMATICS OF THE RIGID 
BODY 
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Figure 4.2.1: Rotation of a body and of an arbitrary a point P about an arbitrary direction [ 


4.1 Introduction 


'The introduction on robot technology has learned us that, from a mechanical point of view, a 
robot manipulator may be regarded as a kinematic chain of rigid bodies articulated together 
through kinematic pairs, or joints. Each body of the system is then subject to translation and 
rotary motions generated by the relative displacements occurring at joints. 


In order to describe the motion of each element in the kinematic chain, from the support to the 
effector, an appropriate formalism is needed. 


One of the crucial aspects to establish such a formalism is the appropriate description of finite 
rotations. The concept of finite rotation will thus be reviewed and revisited in the next sections, 
keeping in mind the specific needs of robotics. 


We will establish next the relationships giving the position, velocity and acceleration of an 
arbitrary point attached to a rigid body which undergoes arbitrary motion. 


Finally, the concept of homogeneous transformation will be introduced to describe the kinematics 
of articulated chains. As it will be shown, homogeneous vector notation allows us to combine in 
one single matrix transformation both rotation and translation operations. It reduces thus any 
operation describing arbitrary rigid body motion to a matrix product. As a consequence, with 
homogeneous notation the kinematics of an open-tree simple structure may be expressed as an 
ordered sequence of matrix products. 


Due to the simplification that it brings into the description of object manipulation, homogeneous 
notation is a computational tool of fundamental importance both in computer graphics and in 
robotics. 


4.2 The rotation operator 


Let Oxyz be a cartesian frame in which the position of a given point P is specified, and let 
us represent the corresponding position vector s in matrix form by the unicolumn matrix s 
collecting its cartesian components. 
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Suppose that a body is rotated of an angle ¢ about the specified direction i. One of its points 
P characterized by an initial position vector sg is moved to a new position whose coordinates 
are 8. 


'The rotation of the body is fully described by frame transformation between the fixed coordinate 
system Oxyz and the body coordinates O'z'y'z', which was initially coincident with the inertial 
frame. 


In matrix form, the transformation from 89 to s may be represented by a linear transformation 
involving a matrix R of dimension (3 x 3) : 


s = RHso (4.2.1) 


4.2.1 Properties of rotation 


Equation (4.2.1) will effectively represent a rotation operation if the length of 3 is preserved. 
This condition may be given in explicit matrix form 


ss = s} RRs 
= SQ So 


and implies thus 
R! R = I. (4.2.2) 


Therefore, the matrix product of an arbitrary position vector s by an orthogonal matrix R 
s = R so with RŒ! = RT 


expresses the rotation of any vector s about a specified direction. 


4.2.2 Remark 


The position vector of P is initially given by Sọ in both coordinate systems. After rotation, 
point P has reached a position that is still given by 5ọ in the body coordinate system, which 
has rotated with P, while the coordinates of P are now s when seen from the initial frame. 

In the following, we will decide to by p and column matrix p the absolute coordinates of 
point P in frame Ozyz after rotation , whereas we will note p' and p’ the coordinates of the 
same point in the relative coordinate Oz'y'z' attached to the body (or body coordinate system). 

After rotation, it comes: 


p= Rp with R' = RT (4.2.3) 


4.3 Position and orientation of a rigid body 
Let us consider the rigid body V of figure 4.3.1 and adopt the following definitions 
- O is the origin of the absolute cartesian frame Oxyz 


- O’ is the reference point attached to body V 
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Figure 4.3.1: Position and orientation of a rigid body 


- O'z'y'z! is a cartesian frame attached to V and centered in O’ 
- F is the position vector of O' relatively to O 
- p is the position vector of P relatively to O 


- p is the position vector of P relatively to O' 


In vector form, the position vector at point P can be decomposed in the form 
p=rtp (4.3.1) 
In order to express equation (4.3.1) in matrix notation, let us represent respectively by 


e p and r the cartesian components of p and 7 in the absolute reference frame Oxyz 


e p! the cartesian components of p’ in the moving reference frame O'z'y' z' 


The frame transformation law giving the cartesian components of P in the absolute frame Oxyz 
takes then the form 
p=r+ Ry (4.3.2) 


where R is the orthogonal matrix describing the finite rotation from frame O'z'y'z' to Oxyz. 
the first term in (4.3.2) represents the translation from O to O', and the second one expresses 
the rotation of the relative position vector p'. 


The relationship (4.3.2) provides the basis of the matrix formalism to describe the kinematics 
of a rigid body. 


4.4 Algebraic expression of the rotation operator 


Various techniques are available to represent the rotation operator R in matrix form. It can be 
expressed in terms of various sets of parameters such as direction cosines, Euler angles, Bryant 
angles, Euler parameters, etc. 
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Figure 4.5.1: Rotation in the Oxy plane 


It is easy to show that an arbitrary rotation may be expressed in terms of a set of three indepen- 
dent parameters. Indeed R is a 3 x 3 matrix containing thus 9 terms, and it can be decomposed 
into column-vectors 

R= [ri T2 r3 | (4.4.1) 


The orthonormality property (4.2.3) has for consequence that the vectors rj are linked by the 
6 constraints. 
rl Tj = Oi (i,j = 1,2,3) 


where ĝ;j represents the Kronecker symbol 


1 if i=j 
oy = 
0 otherwise. 
so that one can write 
R= R(a1, 02,03) (4.4.2) 


where o4, a2, a3 are three independent parameters retained to describe the rotation. Some of 
the possible choices will be described below. 


4.5 The plane rotation operator 


Let us consider the simplest rotation operation corresponding to finite rotation about a coordi- 
nate axis. For example, figure 4.5.1 represents the case where a rotation ¢ is performed about 
the z coordinate axis. 


For a vector r with components r = [x y z] one obtains the change of coordinates 
x = z'cosQ — y'sinó 
y = zx'sinó + y'cosó (4.5.1) 
, 
Z=2 


or, in matrix form 


r= Rr (4.5.2) 
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Figure 4.6.1: Finite rotation in terms of direction cosines 


with the rotation operator 


cos —sing 0 
R(z,$) = sinó coso 0 (4.5.3) 
0 0 1 


Similarly, for a rotation 0 about the y axis and a rotation v» about the x axis, one would obtain 
the rotation operators 
cos? 0 sind 
R(y,0) = 0 1 0 (4.5.4) 
—sinü 0 cos 


and 
1 0 0 
R(x, 4) = 0 cosy -—sinv (4.5.5) 
0 siny cosy 


4.6 Finite rotation in terms of direction cosines 


The most obvious expression for a rotation operator performing an arbitrary rotation is the one 
obtained in terms of direction cosines. To that purpose, let us note (Figure 4.6.1) by (i,j,k) 
and (4 , j', k’) the unit vectors spanning the cartesian frames Oxyz and O'z'y'z'. An arbitrary 


position vector 7 may be expressed indifferently in the Oxyz and O'z'y'z' frames 
r=ai + yj + zk 
= xÙ + yj + z'k' 
Since both sets of unit vectors are orthonormal, a component such as x may be calculated by 


qt = 


ier 
> yy a cr at 
ied zT cT d: y +%-k z 
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and similarly 


y=jr 
=j? +j fy tjk? 
z—k.m 
=k Ťa +k fy + k-k 2! 


^ cos(j, k^) (4.6.1) 


Let us note that when using this representation 


- The dependence of the operator with respect to only 3 parameters is not immediately appar- 
ent; 


- On the other hand, the orthonormality property is obvious since the inverse transformation 
r= R'r (4.6.2) 


is obtained in a similar manner 


L= (Ër t+ Ëy + -kz 
y=- e+ Fy + Ez 
z = (k'r + (kK-y + (K-k)z 


The scalar product being commutative, one easily verifies that 


R = R” (4.6.3) 


4.7 Finite rotation in terms of dyadic products 


A complementary result to (4.6.1) consists to observe that, using matrix notation, any finite 
rotation can be written in the form 


T 


R= 4mm” 4 nn (4.7.1) 


where (l, m,n) and (l',m',n/') are two sets of orthonormal vectors defining two orthogonal 
bases. 
The proof can be established by noticing at first that: 


I — VI^ + m/m" x n n (4.7.2) 
'This can be easily demonstrated by showing that the following relation 


Ia =a 
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holds for any vector a = z'l' + y'm/ + z'm. 


Since 
RT 1 =I 
RT m —m/ 
RT n =n' 


the lemma (4.7.2) can be written as follows: 


I = (RTI) U^ + (R*m) m" + (RTI) n” 
I - R' (i VT+ mm” 4+ nn”) 


which leads to the result of equation (4.7.1) because RT = Rt. 


4.8 Composition of Finite Rotations 


4.8.1 Composition rule of rotations 
Let’s consider two successive rotations: 
e Rotation 1 is the rotation that brings frame Oxyz onto frame Oz1y121, 
e Rotation 2 is the rotation that brings frame Oz1y121 onto frame Ox2y22z2 


The coordinates in the different frame systems are related by the following relations: 


x = Rı xı (4.8 
xı = Rə zə (4.8 
The overall rotations that transforms frame Oxyz into frame Oz2y»29 is given by: 
x = Rı xı = Rı (Rə x2) = (Ri Ro) £2 
because of the associative property of matrix multiplications. Fin ally it comes that 
x = Ra. with R = Rı Rə (4.8.3) 


4.8.2 Non commutative character of finite rotations 


As an example, let us consider an object (Figure 4.8.1) undergoing two successive rotations Fi 
and Rə of 90? about axes z and y respectively 


R, = R(z,90°) 
Rz = R(y,90°) 


One knows that the matrix product is a non—commutative operation 
R, Ro 72 R Rı 


Geometrically, the non— commutativity of finite rotations expresses the fact that reversing the 
order of two successive rotations generates different geometric configurations. 
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x 


y 


in 


Figure 4.8.1: Non—commutativity of finite rotations 


4.9 Euler angles 


Euler angles are the most classical set of 3 independent parameters which allow to express in 
a unique manner the rotation of a rigid body about an arbitrary axis. They are specially well 
adapted to the kinematic description of spinning systems such as tops, gyroscopes, etc. 


The Euler angle formalism consists to decompose the rotation from Oxyz to Oz'y'z' into three 
elementary rotations expressed in body axes (Figure 4.9.1): 


e a rotation ¢ about Oz: R(z,¢) 
e a rotation 0 about Ozi: R(x1, 0) 


e a rotation i? about Oz2: R(z2,~v) 


By combining the 3 successive rotations, the frame transformation can be written 
r = R(z,¢) R(x1,0) R(zo,v) r = Rr' 
with the rotation operator expression 
R = R(z,ó) R(zi,0) R(z2,v) 


and where the elementary rotations about z and æ axes are given by equations (4.5.3) and 
(4.5.5). One obtains explicitly 


cos $ cos i» — sin $cosÜsin? —cos¢dsiny —sin$cos0cosv  sinóosin60 
R = | sindcosw+cos¢dcosdsinw — sin $sin Y + cos cos cosi) — cos ọsin 8 (4.9.1) 
sin ô sin v sin 0 cos v cos 0 
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Figure 4.9.1: Euler angles 


4.9.1 Singular values 


From the very definition of Euler angles it follows that the matrix (4.9.1) becomes singular when 
0 = O0 or m, since both rotation axes along z become collinear. The rotation reduces then to a 
single rotation (à + v) about z. 


4.9.2 Inversion 
Solving the inverse problem consists to deduce the Euler angles (w,0,¢) from the numerical 
values of R given : 
R= T21 T22 T23 = R(wv,0,Q) (4.9.2) 
T31 T32 T33 


let us determine the corresponding values (w,0,¢) of Euler angles. Comparing expressions 
(4.6.3) and (4.7.1) shows that a crude solution would consist of calculating 


= =i x tape OS " -1, 732 
0 = cos (raa), ọ = -— cos (sey Y cos ag) (4.9.3) 


However, this method of solution is not satisfactory for the reasons that an indeterminacy about 
signs of angles remains and that it becomes very unaccurate in the vicinity of singular values. 


In practice an accurate method of solution consists of making a systematic use of the function 


AT AN2 from FORTRAN programming language. The FORTRAN function  — ATAN2(z, y) 


gives y = tan 1(5) and uses the signs of x and y to determine the quadrant in which the 


solution lies. Thus one computes first 
Y = AT AN2(r31, r32) (4.9.4) 


It is then possible to calculate 0 and $ without ambiguity if sin 0 Z 0: 


0 = ATAN2(4/ Tài + 735,733) (4.9.5) 


(0) = AT AN2(r1a, —r23) (4.9.6) 
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Figure 4.10.1: Bryant angles 


Although a second solution exists by using the negative square root in (4.9.5), we always compute 
in this way the solution for which 0 € 0 < m. 


If the solution degenerates (0 = 0 or 7), it is then possible to choose conveniently 0 = ¢ — 0 
and calculate q by 
yY = ATAN2(r21,r11) (4.9.7) 


4.10 Finite rotations in terms of Bryant angles 


In order to represent the orientation of some mechanical systems such as a flying object (airplane, 
robot effector) or devices such as Cardan joints, it is better adapted to define the finite rotation 
operator in terms of three elementary rotations about distinct axes, called roll, pitch and yaw 
(RPY) axes. The use of RPY axes is quite common for task description in robotics. 


Let us decompose the total rotation R into 3 elementary rotations (4.10.1) 
- aw rotation about O;: R(z,v) 
- à 0 rotation about Oy: R(yi,0) 


- a $ rotation about Oxo: R(ax2,¢) 


In terms of these 3 successive rotations, the frame transformation can be written 
r = R(z,v) R(yi,0) R(z2,¢) = Rr’ (4.10.1) 


with 
R = R(z,v) R(yi,0) R(x2, ) (4.10.2) 
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where the elementary rotations about z, yı and x2 are given by equations (4.5.3), (4.5.4) and 
(4.5.5). One obtains explicitly 


cosÜcosi sin@singcosy — sinvcoso sinOcosdcosw + sinvsinó 
R= cos@sinw sinOsinósin«v + coswcosd sinOcosdsinw — singcosw | (4.10.3) 
— sin 0 sin $ cos 0 cos $ cos 0 


4.10.1 Singularities 


A singularity of (4.10.3) occurs when 0 = +4, since axes z and a2 become then collinear. 


4.10.2 Inversion 


'The inversion procedure is the same as the one used for Euler angles 


y ATAN2(ra1, 711) 


0 = ATAN2(—rsi, 4/72, 4 rj) (4.10.4) 


[o = AT AN2(r32, 733) 


I 


Although a second solution exists when taking the negative square root, equation (4.10.1) pro- 
vides a value of 0 such that 


T T 
——«0x— 4.10. 
EET (41905) 
If 0 = +5, we have already indicated that the solution degenerates. Then, we can conveniently 
take ¢ = 0 and 
= ATAN2 if 0= Z 
d M B (4.10.6) 
Y = ATAN2(—r12, r22) if 8 = lr 


4.11 Unique rotation about an arbitrary axis 


Euler’s theorem on finite rotations states that any finite rotation can be expressed as a unique 
rotation of angle ¢ about an appropriate axis e (Figure 4.11.1). 
One numbers then 4 parameters to describe the rotation 


ls, ly, l, and 3) (4.11.1) 


but they are linked by the constraint 


\l| = JBT240 zc: $ € [0,7] 


'The most intuitive procedure to construct the rotation operator in equivalent angle-axis form 
consists to decompose the transformation in three phases 


i) 2 elementary successive rotations (Figure 4.11.2) 
R(z,—o) and = Rly, +8) 


bring the e axis into coincidence with the principal direction Oz; they can be combined 
in a unique rotation 
C= R(y.*B)R(z, —o) (4.11.2) 
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Figure 4.11.1: Expression of a finite rotation as a unique rotation $ about an axis e 


— 


N----- 
J 


Figure 4.11.2: Superposition of two vectors through two successive rotations 
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ii) the rotation ¢ about the Oz axis is described by the elementary rotation R(a, ¢); 
iii) the e axis is placed back into position by the inverse transformation C7! = CT 
'The resulting operation can be expressed in the form 
R(l,¢) = C" R(a,¢) C (4.11.3) 
with the matrix 
cos a cos B sinacos? sing 


C= —sina COS o 0 (4.11.4) 
—cosasin@G —sinasin@ cos 8 


If one notes further that 


sing = l; cos = afte Pe 


l la 
sina = ——4— cosa = ——L —— (4.11.5) 
ae + JG + 8 
we have 
ly ly lL 
Uy 3 i 

C= -4 rs 0 with d= 241] (4.11.6) 

x dM c 


and one obtains the explicit expression for the rotation operator 
i2 Vd + Có lz ly Vo — l: Sọ Il; l; VO + ly SO 


R= |Li,Vóé-LuSó  ÜDVó- Có yl VO — ls Sd (4.11.7) 
lolz VO — 1S0 ll V + l: Sp Vo + Co 


with the notations 
Có = coso, Sd = sing, Vo = wers(Q) = 1—cosd (4.11.8) 
An alternate proof of this result can be obtained by starting from equation (4.7.1) 


/T 


R= lı UU E m m" +nn (4.11.9) 


as follows. Suppose that the rotation $ is performed around the l axis. Then, vectors m’ and 
n’ are transformed according to 


Val 
m = R! m = m cosó — n sing (4.11.10) 
n'— R? n =m sind + n coso 
and substitution of (4.11.10) into (4.11.9) yields to 
R = |li" + cosó (m m? +n n^) + sing (om n? +n m?) (4.11.11) 


If we further note that since (I, rm, m) form an orthonormal basis, the last term represents the 
matrix form of the cross product m x n = l and can thus be written in the form 


nml-mmn'-i (4.11.12) 
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where I is the skew-symmetric matrix 


! 0 -L db 
PSN hL 0 -h (4.11.13) 
SE 0 


The resulting rotation operator is then equivalent to equation (4.11.7), written in matrix form 
R = h IT + cos ó (m m? +n n7) + sing l) (4.11.14) 


Taking into account that 


T 


UT + mm? + nn? = I 


one gets finally 
R= [ cos I + (1—cos¢) LIT. + singi (4.11.15) 


4.11.1 inversion 


In order to invert (4.11.6), let us evaluate the sum of the diagonal terms 


trace( R) Ti1 + T22 + 133 
(i2 4 i + 1?)(1 — cos 9) + 3 cos ó 


1+ 2cos¢ (4.11.16) 


Il 


I 


Il 


Similarly, to obtain the sine of ¢ let us define the vector part of R. 
If ejj; is the permutation symbol such that: 


€123 — €231 — €312 — 1 
€132 = €321 = €213 = —1 
Eijk =O otherwise 


Then 
T32 — T23 
vec(R) = —€ijk Tjk = T13 —131 = 2l sing (4.11.17) 
T21 — T12 


It is then possible to calculate the angle ¢ and the direction of the rotation by 


$ = ATAN2(|vec(R)|, tr(R) — 1) (4.11.18) 
and (R) 
[5 DI (4.11.19) 


One has also 
trace(R) = 1+2cos¢ 


giving 


cosó = sltrace(R) — 1] (4.11.20) 
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4.12 Finite rotations in terms of Euler parameters 


In order to put equations (4.11.6) and (4.11.8) in purely algebraic form, let us define the so-called 
Euler parameters 


dc 
2 
Ae 
2 
eg =l dnd 
y 2 
pas? 
es = Lsinz (4.12.1) 


The rotation operator may then be rewritten in the form 


1— 2(e2 + €2) 2(e1e2 — eoes) 2(e1e3 + €oez) 
R= 2(e1e2 +e0e3) 1-— 2(e?--e$) 2(e2e3 — eoei) (4.12.2) 
2(e1e3 — eoe2) 2(e2e3+e0€1) 1—2(e? + e2) 


or, in a more compact form 
R = (2e9” — 1)I + 2ee” + 2epé (4.12.3) 


where the four parameters introduced are algebraic quantities which play equal roles. They are 
linked by the constraint 
e+e? +e te = 1 (4.12.4) 


Under this form, the problem of kinematic inversion does not give produce singularities. It is 
best performed by observing that the 4 x 4 symmetric matrix S obtained from the elements of 
R 


1+ rir + r22 +733 T32 — T23 T13 — T31 T21 — T12 
S = T32 — T23 ld rir — r22 — r33 T12 T T21 T13 T T31 
T13 — 31 T12 T T21 l — rii + r22 — r33 T32 T T23 
T21 — T12 T13 T T31 T32 T T23 l — rig — r22 + r33 
(4.12.5) 


is a quadratic form of Euler parameters 


eg €o€1  €0€2  €0€3 


2 
€9€1 € €1€2  €1€3 
S=4 1 ; (4.12.6) 
€9€2  €1€2 €z €2€3 
€9€3  €1€3 €2€3 e2 


The knowledge of one row of S' allows us to calculate the parameters eg and e. In practice, the 
algorithm is the following: 


e determine k such that Skk = max;(s;;) 


1 
e compute ej — 5 V Skk 


e compute e; — a i = 0,1,2,3 
€k 
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As it has been demonstrated, Euler parameters form a set of four dependent parameters: 
from a computational point of view, the redundancy contained in their definition is at first sight 
a drawback. However, Euler parameters possess also a certain number of properties which make 
their use very attractive. 


e As it has been seen, the associated inversion procedure never gives rise to singular values; 


e Euler parameters obey to quaternion algebra: as a consequence, successive finite rotations 
may be composed according to the quaternion multiplication rule (see appendix B); 


e Euler parameters are purely algebraic quantities: finite rotations and their numerical 
inversion do not imply transcendental functions. 


It is customary to use Euler parameters to describe the orientation of a robot effector. 
Therefore, they play an important role in trajectory generation methods. 


A more detailed presentation of their properties is given in appendix B. 


4.13 Rodrigues’ parameters 


Let us start again from the fact that the finite rotation of a vector about the origin may be 
described by the operation 


s— Rs (4.13.1) 
with R orthogonal, and that the length of the original vector is conserved 
sT s — ss 20 (4.13.2) 
Or 
(s — s) (s - 8) 20 (4.13.3) 


Equation (4.13.3) means that the vectors f and g defined by 


g—-s-cs' —(R«I)s (4.13.4) 


are orthogonal together 
f'g= (4.13.5) 


Let us next eliminate s’ between both equations (4.13.4). One obtains 
f -(R-I(R-«I)!g-Bg (4.13.6) 
where B is necessarily of antisymmetric type since (4.13.6) yields to 
g' Bg — 0 (4.13.7) 


Let us express this property in terms of the vector b" = [by bz 53] collecting the components of 
the matrix 


; MES b 
B-b-| b 0 -—h (4.13.8) 
-b b 0 


Equation (4.13.6) shows that the rotation operator is such that 
R-I-b(R-I) (4.13.9) 
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or, if we solve with respect to R 
R — (I — 6)! (1 - b) (4.13.10) 
which corresponds to a specific choice of the rotation parameters. 


Let us calculate explicitly 


A = dtm(I — b) = 14- bTb (4.13.11) 
and . " 
(I-b)y! = A [1+ bb? +b] (4.13.12) 
Performing then the product (4.13.9) and using the fact that 
bb = bb" — b bI (4.13.13) 
provides the final expression 
R=I+ rar + Bt) (4.13.14) 


Comparing the results (4.12.3) and (4.13.14), it is easy to check that they are equivalent, pro- 
vided that one takes 


ee ee 
€0 2 
€2 ó 
b» = E = ly tan 2 
b3 = = = I tan 2 
€o 2 
The corresponding vector has the norm 
mm. 
|b| = tan 5 (4.13.15) 


Taking Rodrigues’ parameters in place of Euler parameters offers the advantage of using 3 
independent quantities out of 4 linked by a normality condition. However, they give rise to a 
singularity when ¢ = +7 and have thus to be used with caution. 


4.14 Translation and angular velocities 


Let V be a rigid body undergoing motion (Figure 4.3.1), and let us calculate the velocity of 
an arbitrary point P attached to it. In the absolute frame Ozyz, its position vector has the 
cartesian coordinates 

p =r + Rp’ (4.14.1) 


Where the rotation matrix R may be described using any of the parameter sets just described 
above. 


The components of the velocity vector at point P are obtained through time differentiation 
p = t+ Rp + Rp (4.14.2) 


where 
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- f is the velocity vector of the reference point O’, 


- p' = 0: body V is assumed rigid. 


'The velocity vector is thus given by 
p = t+ Rp (4.14.3) 


and its expression may still be modified by writing it in terms of absolute quantities. Let us 
invert (4.14.1) in the form 
p = Ri(p-r) (4.14.4) 


Equation (4.14.3) may then be rewritten in the form 
p = t+ RRI(p- r) (4.14.5) 
It is easy to observe that (4.14.5) is the matrix equivalent of the vector equation for velocities 


dp dar | A. 4x 
rr qa ^9xG-? (4.14.6) 


where w is the angular velocity of the frame O'z'y'z' relatively to Oxyz. 


Indeed, the orthonormality property RRT = I implies 
RR! + RR. = O (4.14.7) 
from which one deduces that the matrix 
RR? = -RRT (4.14.8) 
is skew symmetric. 


One deduces thus the angular velocity matrix 


i 0 —Uz Wy 
o = RR" = wu; 0 —wr (4.14.9) 
—Uy Wg 0 


where wz, wy and wz are the cartesian components of w in the frame Ozyz. 


Equation (4.14.5) takes thus the final form 
p = t+0(p—r) (4.14.10) 


which is obviously the matrix analog of (4.14.6). 


4.15 Explicit expression for angular velocities 


To the skew-symmetric matrix (4.14.9) let us associate the uni-column matrix w of angular 
velocities 


wh = [We wy we | (4.15.1) 
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Figure 4.15.1: Angular velocities in terms of time derivatives of Euler angles 


4.15.1 In terms of Euler Angles 


In terms of Euler angles, the angular velovity vector results from the superposition of three 
angular velocities (Figure 4.15.1) 
e $ about the O, axis, 
e about the Oz axis, 
e i) about the Oz axis. 
The resulting angular velocity is thus given by 
| ]7 + R9) R(z,0) [0 0 $]^ 


w= [0 0 6] + Riz) [6 00 


The resulting expression of angular velocities is 


ô cos ¢ 4- i) sing sin 0 
w= 0 sino — V cos ó sin 0 (4.15.2) 
o+ v cos 


4.15.2 In terms of Bryant angles 

A similar reasoning leads to a decomposition of the angular velocity into 
e i about the O, axis, 
e Ê about the Oy, axis, 


° d about the Oz» axis. 


The resulting angular velocity is 
=) sin v + db cos ~w cos 0 
u = 0 cosy + siny cos (4.15.3) 
w—¢sind 
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4.15.3 In terms of Euler parameters 


Proof of the result is given in appendix. It presents the remarkable property to provide a bilinear 
expression in Euler parameters and their time derivatives: 


ĉo 
—€1 eo — ea €2 E 
w = 2| —e> es eo -e& = (4.15.4) 
2 
-Ea :—€9 €1 €0 a 
3 


4.16 Infinitesimal displacement 


The concept of infinitesimal displacement allows us to evaluate small position changes at a point 
P (Figure 4.3.1) due to small variations in position r of reference point and orientation R of 
the body frame O'z'y'z'. 


The variation of (4.14.1) provides the expression 
óp = or + óR p! + Rop' (4.16.1) 


The rigid body assumption allows us to omit the last term of (4.16.1), and taking account of 
(4.14.5) provides the expression 


óp = ór--óRR! (p — r) 
Invoking again the orthonormality property of R 
óRR! + R5R’ = O 
tells us that the skew-symmetric matrix 


0 —óo, dQy 
óà = RR? = | da, 0 daz (4.16.2) 
—day day 0 


describes the infinitesimal rotation of O'z'y'z' related to Oxyz. 


The infinitesimal displacement óp may thus be written in a form analog to velocities 


óp = 6r+6a(p—r) (4.16.3) 


4.17 Accelerations 
Expressing the dynamics of a manipulator and planning its trajectory involve necessarily to 
compute the accelerations resulting from the motion. 


In order to express the acceleration at an arbitrary point P of a body V undergoing rigid 
body motion, let us derive (4.13.13) twice with respect to time 


p = *+ Rp’ (4.17.1) 


or, if use is made of (4.14.4) 3 
p = P4 RR” (p-r) (4.17.2) 
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It is straightforward to obtain the meaning of matrix RRT by deriving the expression (4.14.9) 
of the angular velocity matrix w : 


; d A "E 
č = (3) = RR! + RR" 
= ÈR? + 00T 
or .. B 
RR! = -0T (4.17.3) 


where the first term represents the matrix of angular accelerations 


0 ede ùy 
= | & 0 -à (4.17.4) 
-üy ör 0 


ool = w I-ww? 
2 2 
Wy Tw WaWy WW x 
= —wrWy WtW? —uywz (4.17.5) 
2.2 
—U yz Wy We WW, 


Substitution of (4.17.3) into (4.17.2) provides the expression of accelerations 


p = #+(G—G0"')(p—r) (4.17.6) 


4.18 Screw or helicoidal motion 


It is worthwhile noticing that the differential motion of a rigid body as described by equation 
(4.14.5) may be interpreted as screw motion, i.e. the combination of a translation and a rotation 
about a same axis s with arbitrary orientation in space. 


To this purpose, let us determine the locus of points P having a velocity vector p that is 
colinear to the angular velocity w vector. The problem is to find a scalar ø such that 


p = cw =a(p—d)+d (4.18.1) 


Premultiplying by wf provides the expression 


l p; 
gre ou d (4.18.2) 


and the locus itself is obtained by solving (4.18.1) with respect to p. To this purpose, let us 
examine the properties of the solution of the linear solutions of 


x =b (4.18.3) 


It is obvious that a solution to (4.18.3) exists if and only if 


4.18. SCREW OR HELICOIDAL MOTION 


23 


particular solution is obtained through premultiplication of (4.18.3) by the system matrix à 


aaxz = (aa! —||a|?I) v = ab 


Let us try a solution of the form 
x = kab+ypa 


Its direct substitution into (4.18.4) gives 
—k||a|? ab = ab 


ork = Im and the general solution to (4.18.3) is thus 


a ab + 
z= — à pa 
lla]? 


Let us apply the result of (4.18.5) to (4.18.1) rewritten in the form 


G(p-d) = p-d 


Its solution is i 
p-d--jü(p-d)*uw 


or, after noticing that (4.18.1) involves o p = O, one has 


1 
p = d + —30d + pw 
w 


(4.18.4) 


(4.18.5) 


(4.18.6) 


(4.18.7) 


Equation (4.18.7) describes the locus of points having a velocity vector parallel to w. It is a 
straight line s with direction w defined by the parameter u, as it can be very simply observed 


by noticing that two distinct points P, and P5 are such that 
P2— Pi = (ua — )v 
Henceforth we express the locus in the form 


1 . 
WwW 


By premultiplying (4.18.9) by @, we deduce that 


$ lL as 
w(s—d) = G2 wwd 
1 T 2 j 
= geou —w* I)d 
or equivalently 
d—wd = ow-—ws 


(4.18.8) 


(4.18.9) 


(4.18.10) 


(4.18.11) 


Let us finally substitute (4.18.11) into (4.18.1), it provides the following expression for the 


velocity at an arbitrary point P 
p = 7w+ (p-s) 


It is easily seen that it corresponds to a screw motion characterized by 


(4.18.12) 
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Figure 4.18.1: Screw motion 


e A first component of translational velocity parallel to the rotation axis w with a pitch 
velocity given by (4.18.1); 


e A second component resulting from angular motion with rotational velocity w around the 
straight line s which can thus be interpreted as the screw axis of the helicoidal motion 
(Figure 4.18.1). 


The helicoidal representation of motion is the generalization to 3-D motion of the well known 
concept of instantaneous center of rotation: the center of rotation is replaced by the rotation 
axis, and the rotation is complemented by a translation along the rotation axis. 


4.19 Homogeneous representation of vectors 
The homogeneous components of a vector r are the four scalar quantities obtained by adding 
to the three cartesian components [x y z] a scaling factor. 


In homogeneous form, vectors are thus represented in a 4-D space; the 3-D representation 
may be restored by an particular projection. 


Two cases have to be distinguished : the homogeneous representation of a bound vector such 
as the position vector of a point, and that of a free vector such as the velocity or the displacement 
at a given point. 


As an example, let us consider the position vector pı and p» of two point P, and P5 (Fig- 
ure 4.19.1). 
In homogeneous coordinates, a bound vector is presented by the 4-components row matrix 


pl = [z* y^ z* w*] (4.19.1) 


where z*, y*, z* and w* are related to the cartesian components (x, y, z) of the 3-D representation 
by 


gr = — y = — z = — (4.19.2) 


The parameter w* is a scaling factor. One notes in particular that 
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Figure 4.19.1: Position and displacement vectors 


e the homogeneous representation is not affected by a change in scale 


x* 
* 

y 

z* 


Pre gs 


w* 


and it is thus customary to represent a bound vector in homogeneous notation by just 
adding a fourth component equal to 1, leaving then the three first components unchanged; 


e the origin of the coordinate system is represented by the vector 


ul = [0001] (4.19.3) 


Let us calculate next the free vector representing the displacement from P4 to Pj 


T2 — Tı 
d = p- pı = A (4.19.4) 
0 


One notes thus that a free vector is characterized by the fact that its fourth component is zero. 
In terms of homogeneous coordinates, it represents a point at infinity in the d direction. 


4.20 Homogeneous representation of frame transforma- 
tions 


The advantage of using homogeneous representation of vectors both in computer-aided design 
and robotics lies in its ability to represent any frame transformation as an ordinary matrix 
product. 


With homogeneous notation, the translation and rotation terms of the frame transformation 
rule 
p = r+ Rp' 
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Figure 4.20.1: Tool frame transformation .A 


may be gathered in a simple matrix transformation 


p = Ap’ (4.20.1) 
with the 4 x 4 matrix 
R r 
A= | of 1 | (4.20.2) 


Its specific form expresses the fact that it generates rigid body motion. The last row collects 
necessarily three zeros and 


- the upper 3 x 3 block represents the rotation, and is such that RTR = I. 


- the vector r = [rz ry rz] in the last column represents the origin translation. 


Although it is made of 16 terms, the transformation .A depends on only six parameters : 
- the three translations components rz, ry and rz; 


- the three rotation parameters contained in R (Euler parameters, Bryant angles, etc.). 


Let us note that in robotics, it is convenient to build the transformation A describing the 
tool location and orientation in a given reference frame using the four following vectors (Fig- 
ure 4.20.1): 

e r describes the position of the reference point O’ attached to the effector; 

e the approach vector a is adopted as the local z axis; 


e the orientation vector o is the local y axis; 


e the normal vector n, obtained by right-hand rule, gives the local x direction. 
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Figure 4.21.1: Successive homogeneous transformations 


It is expressed in the form 


Ne Or Qr Tr 
Ny Oy Qy Ty 
Nz Og Qz Tz 


0 0 0 1 


(4.20.3) 


In which the first three column are made of the unit vectors n, o and a along the local axes 
O'x', O'y' and O'z’ and the last column describes the position of the effector in the Oxyz frame. 


In term of the vectors n, o and a, the orthonormality property of R can be expressed by 
the six scalar constraints : 


aa^ = nin = olo = 


1 
alo no ola 0 (4.20.4) 


or alternatively, in terms of cross products 


an =o no=a, oa=n (4.20.5) 


? 


4.21 Successive homogeneous transformations 


Let po, pı and p» the position vector of a same point P in successive frames Oozoyozo, O121y121, 
and O»x2y»z9 attached to different bodies of an articulated chain. 


In a general manner, let us define 
^ Aj (4.21.1) 


as the homogeneous transformation matrix providing the cartesian components in frame Oiciyizi 
in terms of its components in Ojzjyjz;. 


In the example described above, we may thus write 


Pi = ‘Ay po (4.21.2) 
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Figure 4.22.1: Geometric representation of a body B’ in body coordinates 


and similarly ; 
po = "Ai pi = "A Ao po (4.21.3) 


'The combined transformation 
0 A> = °A,' A5 (4.21.4) 


expresses thus the transformation from frame 2 to frame 0, and is obtained simply through 
matrix multiplication. 


The same reasoning may be applied in a recursive manner to express that the successive 
transformations °A;, ! A5 ... "7! A, generate the combined transformation 


0 An = Ayo As. A, (4.21.5) 


Equation (4.21.5) provides the combination law for successive homogeneous transformations. 


In kinematic analysis of simply-connected open-tree structures, when the successive frame 
O;zjyiz; are attached the successive links of the system, equation (4.21.5) provides an extremely 
compact representation of the geometrical model of the system. 


4.22 Object manipulation in space 


Let us represent the geometry of an object by the coordinates of a set of points given in a body 
frame. As an example, the prism of figure 4.22.1 may be described geometrically by an array 
B' containing the homogeneous coordinates of the corners (numbered from 1 to 6), expressed 
in a local frame 


1 -1 -1 1 ded 
00 004 4 

Hi per xdv (4.22.1) 
L rto x 11 1 


Supposed that this object is located in the robot environment or has to be grasped by the tool. 
Any manipulation of this object or its description in another coordinate system can be simply 
described by a sequence of homogeneous transformations. 


For example, suppose that it undergoes the following displacements: 
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of 


Figure 4.22.2: Object displacement expressed in global frame 


- a translation of 4 units along the Oz reference axis; 
- a rotation of 90? about Oy; 
- a rotation of 90? about the new Oz axis. 
If one notes 
T(r;,ry, TZ) and R(n, ¢) (4.22.2) 


the homogeneous transformation representing translation and rotation, the resulting transfor- 
mation is : 


1004 0 0 10 0 -1 0 0 
A= 0100 0 100 1 000 
0.01 0 -1 0 0 0 0 0 1 0 
0.00 1 0 00 1 0 0 0 1 
001 4 
1000 
= 0100 (4.22.3) 
0.00 1 
After displacement, body B’ is geometrically described by the array 
B = AB’ (4.22.4) 
or explicitly 
4 4 6 66 4 
1 -1 -1 1 1 -1 
BE 0 0 0 04 4 
1 1 1 11 1 
and its geometric configuration is that of figure 4.22.2. It is easy to verify that the new location 


is the result of the imposed motion. 
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Figure 4.24.1: Closed loop of homogeneous transformations and task description 


4.23 Inversion of homogeneous transformation 
The inverse transformation to (4.20.1) 
p = A'p (4.23.1) 


represents also a change in position and orientation without shape alteration. Therefore, it is 
also of the form (4.20.2) 


R r 
e 
A = E 1 | (4.23.2) 
and has to verify the relationship 
AA =I 

or explicitly 

R r R r RR! Rr +r u I o 

oT 1 o | oT 1 g oT 1 


Simple identification provides the results. 
R = RT 
r = —-R'r (4.23.3) 


Its rotation part is the inverse of the rotation contained in A, while the displacement has its 


sign reversed and is expressed in frame O'z'' z'. 


4.24 Closed loop of homogeneous transformation 


The situation described by Figure 4.24.1 is a usual one in robotics. 
Let us adopt Oxyz as an absolute reference frame and define the following relative transforma- 
tions: 


4.24. CLOSED LOOP OF HOMOGENEOUS TRANSFORMATION 31 


- Z : position and orientation of the manipulator support in Oxyz frame; 
- ZT : position and orientation of the wrist in frame Z; 

- TE: frame attached to end of tool relatively to wrist attachement; 

- B : location and orientation of body B in Oxyz frame; 


- PG : grasping transformation, describing location of grasping point and grasping orientation. 


Figure 4.24.1 shows that two different points of view may be adopted to describe the grasping 
task: 


e from the "robot" point of view, the grasping transformation is 


E-Z^TTE (4.24.1) 


e from the "world" point of view, the task to perform is 


E = B?G (4.24.2) 


Identifying the two points of view gives rise to the matrix identity 


Z^TlE = B.PG (4.24.3) 


Equation (4.24.3) can than be resolved with respect to any of its components. For example, 
given the task description and the location of the manipulator, the robot configuration may be 
computed from 

7p  Z C, B. PG. E (4.24.4) 


Equivalent form of closed-loop equations such as (4.24.3) and (4.24.4) are all contained in a 
transformation graph (Figure 4.24.2) where 


e each node represents a specific frame; 


e each branch represents a direct transformation if traveled in the positive direction and its 
inverse if traveled in the negative direction. 


On the diagram of Figure 4.24.2, one easily verifies 


e equation (4.24.3), when traveling from the reference frame to the tool frame along two 
distinct paths; 


e equation (4.24.4), when proceeding in the same manner from the manipulator base frame 
to the wrist frame. 


'The transformations T' and E added on the diagram represent the wrist and the tool frames 
in absolute coordinates. 
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absolute frame 


object frame 
robot base frame 


wrist frame = effector frame 


Figure 4.24.2: Graph representation of frame transformations 


4.25 Homogeneous representation of velocities 


It is possible, through time differentiation of equation (4.19.1), to express transformation and 
angular velocities of a given point P in a single matrix transformation. Let us suppose that P 
is attached to a rigid body so that its velocity p' relatively to O'z'y'z' is zero. Differentiation 
provides the free vector 


P = [pe by b0] = Ap’ (4.25.1) 
where 
i, [R f$ 
A= | oT 0 | (4.25.2) 


is the matrix of angular velocities. 


It may still be expressed in the form 
å = AA (4.25.3) 
where A is the velocity differential operator such that 
p = Ap (4.25.4) 


Equation (4.25.4) is the analog of (4.14.5) written in homogeneous coordinates. One finds 
explicitly 


À = Å A” = E d (4.25.5) 


where à is the angular velocity matrix (4.14.6) and v represents the velocity of a point coinciding 
instantaneously with the origin of Ozyz: that is p — o. The velocity vector is thus 
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4.26 Homogeneous representation of accelerations 
A second differentiation of equation (4.23.3) provides the free vector of accelerations 
P = [Be By B. 0)" = Ap’ (4.26.1) 
where ty | 
A= | E ^ | (4.26.2) 
Matrix (4.26.2) may also be put in the form 
A=AA (4.26.3) 


where the acceleration differential operator A is such that 


p = Ap (4.26.4) 
One finds explicitly 
.. B a 
AE E : | (4.26.5) 
where . 
B = RR! = -00T (4.26.6) 


collects angular and centrifugal accelerations and 
T), (4.26.7) 


represents the acceleration of a point attached to the body which coincides instantaneously to 
the origin of the Oxyz frame. 
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Chapter 5 


KINEMATICS OF SIMPLY 
CONNECTED OPEN-TREE 
STRUCTURES 
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5.1 Introduction 


'The kinematic model concept has already been introduced for planar kinematic structures. It 
expresses the algebraic relationship between configuration space, defined by the m joint variables 
q(t) = [m ... q]" and task space, defined by a set of parameters æ(t) = [r1 . .. £n]? describing 
position, orientation and status of the effector. 


'The bilateral relationship 
x = f(a) (5.1.1) 


existing between configuration space and task space is verified in a trivial manner when taken 
in the direct sense, but solving the inverse problem raises a certain number of difficulties such 
as condition of existence, its multiplicity and the effectiveness of the available methods to solve 
it. 

'The goal of this chapter is to proceed to the construction of the kinematic model for a 3 — D 
manipulator with open-tree, simply connected mechanical structure. The type of joints consid- 
ered are all of class 5 (one DOF) since only these are used in open-tree kinematic architectures. 


'The homogeneous transformation formalism is adopted as representation tool since it pro- 
vides an easy expression of the successive transformations occurring in a kinematic chain. 


At an elementary level, one will consider two distinct methods to express the geometric 
transformation introduced by one link within a kinematic chain. 


e The most classical one is the Hartenberg-Denavit (DH) representation. It presents the 
advantage to describe most situations encountered in robot architectures while using a 
very limited number of geometric and displacement parameters; 


e The Sheth method is much more general, but is not so flexible to use because of its large 
number of parameters. 


We will describe both formalisms in order to indicate their respective limitations, but later 
on we will limit ourselves to the use of the DH notation. 


It will next be shown how the homogeneous transformation formalism can be applied to solve 
the inverse kinematic problem in closed form, provided that the uncoupling condition between 
effector translation and orientation displacements is verified. 


The rest of the chapter will be devoted to the construction of the differential model and its 
use. 


5.2 Link description by Hartenberg-Denavit method 


Let us consider a binary link of an articulated mechanism such as represented in Figure 5.2.1. It 
establishes a rigid connection between two successive joints numbered n and n+1, and their axes 
are supposed skew. Its geometry can be described very simply in terms of only two parameters: 


e The distance an, measured along the common perpendicular to both axes; 
e The twist angle an, defined as the angle between both joint axes. 


If on the other hand, relative motion is restrained to joints of revolute, prismatic, cylindrical 
and skew types, the relative displacement occurring at joint n may also be described in terms 
of two parameters: 
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Joint n+l 


Joint n 


Figure 5.2.1: Hartenberg-Denavit representation of a binary link 


e The rotation 0,, about the joint axis; 


e The displacement dn along the same axis. 


The geometric parameters of the member 


A link n is connected to, at most, two other links (i.e. link n — 1 and link n 4- 1). Thus two 
joint axes are established at both ends of the connection. The significance of links, from a 
kinematic perspective, is that they maintain a fixed configuration between the joints which can 
be characterized by two parameters an and o, which determine the structure of the link. They 
are defined as following: 


e an, the length of the member, is the shortest distance measured along the common normal 
between the joint axes ie. Z,_1 and z, axes for joint n — 1 and n, 


e an, the twist angle, is the angle between the joint axes measured in a plane perpendicular 
to common perpendicular. 
The displacement parameters of the member 


A joint axis establishes the connection between two links. This joint axis will have two normals 
connected to it, one for each link. The relative position of two such connected links is given 
by d, which is the distance measured along the joint axis z,_; between the normals. The 
joint angle 0,, between the normals is measured in a plane normal to the joint axis. Hence, the 
parameters dn and În are called distance and angle between adjacent links. They determine the 
relative position of neighboring links. 


The different types of joints which can be represented correspond then to the following cases: 
- Revolute joint: On = 0,(t), while d, is a fixed length. 
- Prismatic joint: dn = d, (t), while the angle 0,, remains constant. 


- Cylindrical joint: On = 0,(t) and dn = d, (t) may vary independently. 
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Joint n Joint nel 


Joint nej 


Bn.) Link n-I 


Link n-2 


Figure 5.2.2: Hartenberg-Denavit frames on a kinematic chain 


- Screw joint: On = 6,(t) and dn = k,0,(t), where kn is the constant pitch of the screw. 


Considering next an articulated chain of such members, let us define a frame O2rnYnZn 
attached to link n as follows (Figure 5.2.2): 


e Its origin On is located at the intersection of joint axis D,+41 with the common perpendic- 
ular to Ln and L444; 


e The zn axis is taken along the common perpendicular; 
e The zn axis coincides with joint axis Ln+1; 


e The yn axis is defined by the cross product Yn = Zn X Ln. 


The frame transformation "~1A,, describing the finite motion from link n to link n — 1 may 
then be expressed as the following sequence of elementary transformations, starting from link 
n-i: 


1. A rotation 6, about z4.1; 
2. A translation d, along the same axis z, 1; 
3. A translation a, along the x, axis; 


4. A rotation a, about xp. 


Steps 1 and 2 describe the relative displacement at joint n, while steps 3 and 4 describe the 
frame transformation associated to link geometry. 


In matrix form 


n-lA,— Rot(zj, 1,04) : Trans(Zn-1, dn) + Trans(a@n, an): Rot(z,,,o) (5.2.1) 
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P 


ay 
$ 
Figure 5.3.1: Open-tree simply connected structure 


and its explicit calculation provides the expression 


cosÜ,  —sinO,coso,  sinÜ,sino, an Cosby, 
E sin, | cosO,coso,  -—cosO0,sino, a,sinÓ,, 
n-lA, = . (5.2.2) 
0 Sin Qn, COS Qn, dn 
0 0 0 1 


If the description of articulated structures is restricted to open-tree simply connected struc- 
tures with joints of class 5, then the local frame transformation (5.2.2) may be expressed as a 
function of one single DOF 


TA, =" An (qn) (5.2.3) 


5.3 Kinematic description of an open-tree simply connected 
structure 


Let us denote by T the homogeneous transformation describing the position and orientation of 
the tool for an open-tree simply connected structure as represented in Figure 5.3.1. 


On one hand, it results from the successive transformations introduced by the individual 
links. Each transformation involving only one DOF, one may write 


tii(q1,---Qm) sea dika tya(qi,---Qm) 

= toi(@i,---Qm) «+--+ tea(Qi,---Gm) 
T =° A; A2.. Tt Am = | ub i 5.3.1 
k E: t31(q1;---qm) aax xx t34(q1;---qm) ( ) 


0 0 0 1 
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On the other hand, the expression of T' in task space is 


Ng Or Ax Tg 


| ny Oy ày Ty | | R m 
ax Nz Og Gz Te | | o? 1 | 15:9:2) 
0 0 0 1 


where n, o, and a represent respectively the normal, approach and orientation vectors of the 
tool and r, the location of its center. 


In order to apply Hartenberg-Denavit's method to a simply connected open-tree structure, 
let us identify first the joint axes L, and attach to them a variable o, describing their type: 
since one is limited to R and P joints, let us write for each joint axis Ln 


PR 0 fora teo jou (5.3.3) 
1 for a prismatic joint 
in which case the associated DOF is 
Qn = (1 — 04)05 + Ondn (5.3.4) 


5.3.1 Link coordinate system assignment 


The geometric kinematic model may then be obtained by applying the following recursive pro- 
cedure. 


For each link, when traveling from base to effector of the manipulator, the intermediate 
transformations "~!A,, can be identified in four steps: 


1. The origin O, of frame O,z4,ysz& is the intersection of the common perpendicular to Ln 
and Ln+ı with axis Ln+1. In the case of occurrence of two successive axes which are 
parallel or confused, the location of the common perpendicular has however to be chosen 
arbitrarily. 


2. Axis Zn is defined by the unit vector along Ln+1: it is arbitrarily oriented. 


3. Axis x, is the unit vector along the common perpendicular to Ln and Ln+1, and directed 
towards L441. When axes Ln and L444 intersect, then the choice of the x, can not be 
determined in that way. From the definition of the Denavith-Hartenberg transformation, 
Zn—1 comes into coincidence with zn by a rotation around £n. So £n is the common 
perpendicular direction to Ln and L,4,, and it comes that £n must be always be chosen 
as the cross product Znņn—1 X Zn. 


4. Axis Yn is defined by the cross product rule y, = Zn X £n. 


Remarks 


e At the effector, the vector z, and the joint axis L are oriented along the approach vector a. 
However for planar manipulators, it is generally much simpler to create a virtual joint axis 
at the effector that is out of the plane of the manipulator than in the approach direction 
as for space manipulators. 


e One is free to choose the location of the frame Ooxoyozo anywhere in the supporting base, 
as long as zo axis lies along the axis of motion of the first joint. 
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Wrist rotation 


shouder rotation 


26.5 fn. 


flange rotation 


Wrist rotation 


Figure 5.4.1: kinematic architecture of 6R type 


e When joint axes of Ln and Ln+ı are parallel, it is better to take On in such a way 
that the distance dn is minimum (often zero) for the link for which the origin is defined 
unambiguously. 


e At effector level (link n), the origin of the coordinate system is generally chosen identical 
to the origin of the link n—1. So °Ag¢ specifies the position and orientation of the end-point 
of the manipulator with respect to the base coordinate system. 


e If the manipulator is related to a reference system by a transformation B and has a tool 
attached to its last joint’s mounting plate described by H, then the end-point of the tool 
can be related to the reference coordinates frame by 


refT oi = B "As H (5:3:5) 


5.4 Geometric model of the PUMA 560 


Let us consider the structure of the PUMA robot described by Figure 5.4.1: it has an open-tree 
architecture of type 6R. 


Its kinematic architecture is represented schematically in Figure 5.4.2 in a specific configu- 
ration which we use as a reference. The figure displays 


- The definition of the joint axes Lı to Ly, Ly being a supplementary axis defined at the end 
of link 6 and chosen coincident with the approach vector; 


- The points O; at frame origins which can be defined without ambiguity. 


Figure 5.4.3 represents the frame vectors £n and Zn which can be defined without ambiguity, 
except for their direction. It is easy to complete the frame definition by choosing the frame 
centers Oo, O2 and Og and making an appropriate choice for the directions £o, x4 and £e 
(Figure 5.4.4). 
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Figure 5.4.3: Location of axes £n and Zn 


(00.01) ; A (0203) 2 ee 
A RT 0) 2 
= ~ Og). 
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XX; Zs "eb. 
m Xs 


Figure 5.4.4: Complete definition of frames O,z,ynzn 


5.4. GEOMETRIC MODEL OF THE PUMA 560 9 


Tink 6 


am 
qı 
T 


Ta9.5mm [0 | 
qx valuc [9 a 9 | 


Variation | —160° —225? —45° —110° | —100° —266° 
160° 45° 225° 170° 100° 266° 


[te | | 
[on | -$2 | 
| 08 | 0 | 
^n | a | 
a SH 
| am value | 3 | 


Figure 5.4.5: Hartenberg-Denavit representation of the PUMA 560 


The Yn vectors are such that Yn = Zn X £n and are not represented in the figure. The 
corresponding parameters in Hartenberg-Denavit notation are given for the PUMA 560 by the 
Table 5.1. 


The successive transformations have the following expressions 


Ci 0 — Sı 0 C» — S3 0 a2 C5 
S 0 C, 0 Bac. 0 uas 
0 EE 1 1 1 I 2 2 2:22 
Ai 0 -1 0 0 AP | d 4 do 
Ü 0 0 1 0 0 0 1 
C3 0 $3 0 C4 0 —94 0 
à S3 0 eC ae a 0 Cp. 0 
a=) 5 T 9 ^ AIO ux Cd. d eal) 
0 0 0 1 0 0 0 1 
Ce 0 % 0 Cs e 0 0 
44 | Ss 0 -0O 0 e... Nive Ce. 0 0 
As 0 1 0 0 As=| 9 p 1 de 
0.0 0 1 0 0 0 1 
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To express the complete transformation 
T =° A, -1 Ap 2 A3 3 A4 -4 As ? Ag (5.4.2) 
describing the effector configuration, we use the notations 


Cij = cos(6; + 0; eps .) (5.4.3) 
Sij = sin(0; + 0; +.. .) 


and we express the successive matrix products, starting from the effector, to obtain the inter- 
mediate matrices 

C5Ce —CsS6 Ss Ssde 
S5Ce —S5S6 Cs Csde 


44. 4A. 54. — 
Ac =! As ^ Ag NR UE E (5.4.4) 
0 0 0 1 
C4C5Cg — SaSg —C4C596 — S4C6 | C455 C4S5d¢6 
$4C5C6 +C4S6 —S4Cs S6 +C4Ce S45 S4S5d 
T TRE NEC AE 4C5C6 + C496 4C5 S6 + CaCg S495 45S5d6 
Ag Aae Aa S555 PNE (et) 
0 0 0 1 
“Ae ="A3 ? Ag 
COC 08.) OKC AEOD OSC... CC 
- 556, 4-555555 485(da + Csde) 
_ | S3(CaCsCe — S486) —S3(C4C5S6 + 94C6) S$3C4S5 — C3C5 S3C4S5d¢6 
£0,520; Cees: Osi Code) 
S4C5C6 + C456 —54C5656 + C4C6 S455 S4S5dg 
0 0 0 1 
(5.4.6) 
TAs —lA, 2 Ao 
Co3(C4C5C6 — S496) —Co3(C4C56 + $406) C2301495 C53C4 55d 
—5355C6 -F55355 56 -S33C5  +S23(da + Cade) 
+a2C2 
= | S23(C4C5C6 — $495) —So3(C4C5S6 + $4C6) So3CaSs5  S$23C4S5d6 + a252 
+C23 S5 C6 — C23 S5 S6 —C23C5 —C23(d4 + Csde) 
S4C5 C6 + C456 — 94C5596 + C4C6 S455 S4S5dg + do 
0 0 0 1 
(5.4.7) 


Let us note that in (5.4.7) the angles 09 and 63 are simply added since they correspond to 
rotations about parallel axes. 


The final result is 


T =° A; Ag = °y (5.4.8) 
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with the components 


n4 = C1|Cos(C4C5C6 — S456) — S2385C6] — S1(S4C3C6 + C456) 
ny = S1[C23(C4C5C6 — S496) — S2385C6] + Ci(S4C5Ce + C496) 
Nz = —S33(C4C5C6 — S496) — Co355Co 

Oz = Ci1[- Cos(C4C5Ss + S4C6) + 235595] + $1(54C556 — C4Cs) 
Oy = $31[- Cos(C4C5Ss + SAC) + S238596] — C1(S4C596 — C4Cs) 
0; = S33 (C4C596 + $4Cs) + C2355 S6 (5.4.9) 
Ag = Ci(Co3C4S5 + S23C5) — 515495 

ay = S1(C23C4S5 + S2305) + C1 S455 

az = — S230455 + C23C5 

rz = Ci[C53C4S5dg + S23(d4 + Csdg) + a3C3] — S1(S4S5ds + d2) 
ry = 54[C23C4S5dg + S23(d4 + C5dg) + a2C2] + C1(S4S5de + d2) 
T; = (—S23C4S5 + C23C5)de + Co3d4 — a2S2 


Note that in the introduction on robot technology, we have mentioned that a particular archi- 
tecture where the wrist is made of three revolute joints with intersecting axes and orthogonal 
two by two provides decoupling between effector location and orientation. The point of inter- 
section of the three joints at wrist level behaves like a virtual spherical joint and its location is 
determined by the only three displacements 01, 62 and 63. 


The PUMA 560 architecture takes advantage of this interesting property. The virtual spher- 
ical joint may then be defined as the specific point obtained by setting a zero effector length 


dg =0 (5.4.10) 
in which case the formulas (5.4.9) describing the effector position simplify into 


ri = C5 (S»3d4 + a2C2) P Sido 
P = Sı(S23d4 + a202) + Cido 
r, = Co3d4 T à2 95 (5.4.11) 


where r'/ is the modified position vector 


r—r-—dg;a (5.4.12) 


'The geometric model splits then into two sets of equations which we may formally write in 
the form 


r' = f (61, 62,43) 
R= R (61, 62, 63, 04, 05,06) (5.4.13) 


5.5 Link description using the Sheth method 


The Hartenberg-Denavit method that we have just introduced to describe kinematic chains 
suffers from several drawbacks: 
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Figure 5.5.1: Description of ternary link by Hartenberg-Denavit's method 


1. The successive coordinate axes are necessarily defined in such a way that the reference 
point On and the axis £n are defined on the common perpendicular to adjacent link axes. 
From the point of view of data generation, this may render difficult the task of specifying 
the geometry of the link and its structural data. 


2. The DH transformation mixes two types of information: a, and a, describe the link 
geometry while 0,, and dn are motion-related quantities. 


3. The DH notation cannot be extended to ternary links. 


Let us take the example of a multiply-connected system, which involves then one or more 
ternary links (Figure 5.5.1). With the DH notation, the displacement parameters 0; and d; of the 
link cannot be defined in a unique manner: according to Figure 5.5.1 two distinct displacement 
parameters have to be introduced such that 


oP = 9 4 9 (5.5.1) 
d? — D + ys (5.5.2) 


where $; and w; are additional geometric parameters describing the link. On the other hand, 
the geometric parameters a; and a; characterize the previous link. They are thus not modified. 

Sheth's method overcomes the limitations due higher order links by introducing a number of 
frames equal to the number of joints on the link. At the same time, it provides more flexibility 
to specify the link geometry. 


With Sheth's method, the geometry of the link (Figure 5.5.2) is specified by the position and 
orientation of frames attached to each joint. Figure 5.5.2 shows the case of a binary link where 
a first frame u;jv;w; is attached at the 'origin' of the link (joint j) and a second one, z&yx zx, 
to the ’end’ (joint k). The expressions ’origin’ and ’end’ result simply from the traveling path 
in the kinematic chain. 


To describe the geometry, one locates first the common perpendicular to both joint axes wj 
and zy. One chooses then arbitrarily on it a positive direction with unit axis t;;. 


Specifying the link geometry requires no less than 6 parameters which are however easily 
found as follows (Figure 5.5.2): 
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Figure 5.5.2: Sheth’s description of a binary link: (a) definition of frames attached to the link; 
(b) definition of geometric parameters 


ajk is the distance from w; to zk measured along tjg. 


e bj, is the distance from tjk to x, measured along zi. 


Cj is the distance from uj to tj; measured along wj. 
* ajk is the angle made by axes w; and zz, measured positively from w; to zę about t;;. 


e Bix is the angle made by axes tjk and £k, measured positively from tj, to x, about the 
Zk axis. 


e 7/j; is the angle made by axes uj and tjg, measured positively from uj to tj; about wj. 


'They generate a geometric transformation which can be noted 


Gin = G(ajk, bjk, Cjk, Qjk, Bjk, Vjk) (5.5.3) 
It is such that 
uj Tk 
% | Ga | Jk 
w (TRE a (5.5.4) 
1 1 


The relative motion produced by the joint axis j is treated independently. It generates a 
displacement transformation which is supposed to depend on one single joint DOF q; 


D; = D;(q;) (5.5.5) 
and such that 
Tj Uj 
Yj —p».| “3 
Ai Dj n (5.5.6) 
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For a ternary link, several geometric transformations (5.5.4) have to be generated but the 
displacement part (5.5.5) of the transformation remains unique. 


The resulting frame transformation from frame x,y,z, to frame 25yjzj is then calculated 
by 
Ajk = DjGjk (5.5.7) 
The advantages of this notation where geometric and relative motion transformations are treated 
separately are the following: 


1. For ternary links connecting joint j to both joints k and l, two geometric transforma- 
tions Gj, and Gj, have to be introduced but the displacement part remains common. It 
generates two transformations 


A jk = DjGjk and Aj = D;Gji (5.5.8) 
2. Both coordinate systems (u, v, w) and (x, y, z) attached to extremities of the link are 


defined quite arbitrarily. An adequate choice often simplifies a lot the geometric description 
of the link, and may also make the description of the kinematic model easier. 


3. The geometric transformation is specific of the link under consideration, while with the 
DH method it involves quantities attached to the previous link. 


Sheth's notation allows to describe very simply a large number of joints such as not only 
revolute and prismatic pairs, but also cylindrical, screw, spherical, gear pairs ... 
5.6 Sheth’s geometric transformation 


According to the notations adopted, and omitting the subscripts j and k for sake of clarity, the 
geometric transformation from origin to end of the link is 


u 
: —T'rans(0,0, c) - Rot(z, y) 
1 
x 
T'rans(a, 0,0) - Rot(z, o) - Trans(0, 0, b) - Rot(z, B) , (5.6.1) 
1 
One obtains the explicit expression of the transformation matrix 
cos 3 cos y — sin B cos y sin a sin y à COS ^y 
—cosasin@siny — coso cos f sin y +bsin o sin y 
cos B sin y — sin B sin y — sinacos^y a sin y 
G= ? : (5.6.2) 
+cosasin cosy -+cosacos f cosy —bsinacosy 
sin asin 8 sin a cos 3 cosa c+bcosa 


0 0 0 1 


One can show that in the general case, equation (5.6.2) is easily evaluated from the knowledge 
of the coordinates of six points attached to the link. 
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Figure 5.7.1: The revolute pair 


5.7 Relative motion transformations 


The explicit expression of matrix (5.6.2) depends upon the type of kinematic pair considered. 


5.7.1 The revolute pair 


The frame definition is such that 
- axes z; and w; coincide with the rotation axis, 
-both frames have same origin. 


The displacement matrix is then 


cos#; —sin0; 0 0 
sind; cos@; 0 O0 

D;(0;) = 0 0 : 10 (5.7.1) 
0 0 0 1 


5.7.2 The prismatic pair 


The only variable is the displacement d; along the joint axis, and the displacement transforma- 
tion is expressed according to the following conventions: 


- axes wj and zj coincide with the translation direction; 
- axes uj and x; are chosen parallel and noted positively in the same direction. 


The transformation matrix is then 


Dj(dj) = (5.7.2) 


ooor 
oorno 
onoo 
races 
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Figure 5.7.2: The prismatic pair 


Figure 5.7.3: The cylindrical pair 
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Figure 5.7.4: The screw pair 


5.7.3 The cylindrical pair 


In this case, the joint numbers 2 DOF: rotation and translation about the same axis. It can be 
described by a transformation combining one revolute and one prismatic joint. The transfor- 


mation matrix is 


cos; —sin0; 0 0 
sinĝ; cos0; 0 0 

D;(0;, dj) = 0 m^ -i d (5.7.3) 
0 0 0 1 


The only condition for a correct use of (5.7.3) is that axes z; and w; have to be aligned along 
the joint axis. 


5.7.4 The screw joint 
When the displacements 0; and d; of a cylindrical joint become proportional, the cylindrical pair 


degenerates into a screw pair. The relationship between 0; and d; reduces to one the number 
of DOF of the pair; it is expressed in terms of the lead of the screw, defined by 


2nd; 
da (5.7.4) 
0j 
The transformation may be indifferently expressed in terms of the relative rotation 0; or dis- 
placement d; 


cos; —siné; 0 0 

sinf;  cos0; 0 0 

0 0 0 1 

or 
cos TA — sin (74) 0 0 
n 21d; 2nd; 0 0 
Thus | ELE. COS UTE ) (5.7.6) 

0 0 1 dj 
0 0 0 1 


Both transformations can be used under the following restrictive conditions: 
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Figure 5.7.5: The gear pair 


- Axes w; and z; have to be aligned along the screw axis; 


- Frames ujvjwj and x;y;2; have to be chosen in such a way that the axes uj and x; coincide 
in a given reference configuration. 


5.7.5 The gear pair 
'The gear pair is chosen as an example of how the extended notation may be used for higher pairs. 
The frames u;jv;w; and z;y;z; are attached respectively to links G and H in their rotation axis. 


The common perpendicular to axes zj and w; is the t axis; then, the general transformation 
G of equation (5.6.2) may be used to describe the displacement rotation introduced in the gear 
pair. 


With the choice of parameters 


a = Rı + Rs, a=b=c=0, y= b, DB — 05 (5.7.7) 
and the definition of the gear ratio 
Ry 
Se 5.7.8 
xo (5.7.8) 
substitution into (5.6.2) provides the explicit expression 
cos((14-7)01] -—sin[((1--7)04] O (1-4 7)Hacos0; 
D,(6) = sin((14-7)061] cos|(1+7)ðı] 0 (1+7)Rosin6) (5.7.9) 
0 0 1 0 
0 0 0 1 


5.8 Sheth’s description of the PUMA 560 


The application of Sheth’s formulation to describe the kinematics of an open-tree simply con- 
nected structure is relatively straightforward. As an example, consider the PUMA 560 structure 
of section 5.4. 


5.8. SHETH'S DESCRIPTION OF THE PUMA 560 


Us 


Link 4-5 


Link 5-6 Link 6-7 


Figure 5.8.1: Successive frames in Sheth’s representation of the PUMA 560 
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Table 5.2: Geometric parameters for PUMA robot 560 


'The first step consists of breaking the structure into individual components and attaching 
to each of them origin and end frames u;v;w; and £kYkzk. The axes wj and zę are each time 
taken into coincidence with joint axes. 


It is easy to verify that the geometric parameters describing each of the members are those 
given in the table of Table 5.2 
The resulting transformations are 


0 OE ay 1 0 0 æ 
0—1 0 0 010 0 
G2=|i o 0 d G»=]| 9 9 1 0 
0 0 0 1 000 1 
00 1 d, 0100 
100 0 0010 

Gu=|q 10 0 Gs=|i 9 9 0 Qa) 
000 1 0001 
0010 100 0 
1000 010 0 
GES g 10 0 Gu 0 0 1 de 
00 0 1 000 1 


5.9 Inversion of geometric kinematic model 


One has already mentioned that the problem of solving the kinematic equations of a manip- 
ulator is a highly nonlinear problem. These equations express, as depicted by figure 5.9.1, a 
correspondence between joint (or configuration) space and task (or operational) space. 


The number of variables in joint space is equal to the number m of DOF occurring at the 
joints of the system. In particular, for a simply-connected open-tree structure will all joints of 
class 5, this number equals the number of links. 


The number of variables in task space is equal to the number of DOF of the task. Arbitrary 
location and orientation of the tool implies the definition of 6 parameters is task space. Spe- 
cific tasks such as vertical insertion, manipulation of cylindrical objects, use of rotating tools 
(grinders, screw drivers ...) can be defined with a smaller number of DOF. 


Given the joint variables (q1, ... qm), the equation 


x= f(q) (5.9.1) 
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Inverse Problem 
JOINT SPACE * TASK SPACE 
r x — f(q) 
q -[q ...q] " x =[x ...x] 
Direct Problem 


Figure 5.9.1: Nonlinear mapping by the geometric kinematic model 


has always one and only one solution since the functions fT = [fi ... fm] may always be ex- 
pressed in an explicit manner. The direct problem is thus trivial. 


'The problem of solvability arises when considering the inverse problem: given the position 
and orientation of the tool in task space, which set of joint displacements actually generates 
such a configuration? 


Since the functions (f1,... fm) are of transcendental type and are thus highly nonlinear, 
we must concern ourselves with the existence of solutions, their multiplicity and the methods 
available to reach them. 


Existence of solutions 
'The question of whether solutions exist or not raises first the question of manipulator workspace. 


When examining the kinematic behavior of planar structures, we have already defined 
workspace as the space which the end-effector or the manipulator can reach. The definition 
may be improved further by distinguishing between 


e dextrous workspace, as the volume of space that the end-effector can reach with all orien- 
tations. 


e position workspace, as the volume of space that it can reach in at least one orientation. 


Workspace is limited by the geometry of the system on one hand, and by the mobility allowed 
to the joints on the other hand. 


The existence of solutions depends also on the number of degrees of freedom of the system. 


e The normal case is when the number of joints equals 6. Then, provided that no DOF 
are redundant and that the goal assigned to the manipulator lies within the workspace, 
solutions normally exist in finite number. The different solutions correspond then to 
different possible configurations to reach the same point. The multiplicity of the solution 
depends upon the number of joints of the manipulator and their type. The fact that a 
manipulator has multiple solutions may cause problems since the system has to be able 
to select one of them. The criteria upon which to base a decision may vary, but a very 
reasonable choice consists to choose the ‘closest solution to the current configuration. 


e When the number of joints m is less than 6, no solution exists unless freedom is reduced in 
the same time in task space, for example by constraining the tool orientation to certain di- 
rections. Mathematically, it is always possible, given an arbitrary position and orientation 
of the tool, to find a robot configuration which brings the tool in the ‘closest configuration 
to it. This resulting configuration will then depend on the criterion adopted to measure 
proximity. 
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e When the number of joints m exceeds 6, the structure becomes redundant and an infinite 
number of solutions exists then to reach the same point within the manipulator workspace. 
Redundancy of the robot architecture is an interesting feature for systems installed in a 
highly constrained environment. From the kinematic point of view, the difficulty lies 
in formulating the environment constraints in mathematical form in order to insure the 
uniqueness of the solution to the inverse kinematic problem. 


5.10 Closed form inversion of PUMA 560 robot 


5.10.1 Decoupling between position and orientation 


The very first step in order to obtain a closed form solution to the geometric kinematic model 
of the PUMA robot displayed in figure 5.4.1 is to define the position of the end-tool frame in 
such a way that uncoupling occurs between position and orientation. 


Let us express in global coordinates the location of the wrist center 
p=Ta with a’=[(00 — de] (5.10.1) 


where dgis the length of the tool. The resulting point is located at the intersection of the wrist 
axes z4, Z5 and ze, and substitution of (5.10.1) into (5.4.8) provides the new translation vector 


C4 (So3d4 RUE a2C2) — Sidz 
p= | S1(So3d4 + a2C2) + Side (5.10.2) 
Co3d4 — a2S2 


By gathering the rotation part of (5.4.9) and the translation vector (5.10.2), a new transforma- 


tion can be defined 
|| R p 
qu | 0 1 | (5.10.3) 


which verifies now the uncoupling property. 


5.10.2  Pieper's technique 


In order to perform the closed-form inversion of (5.10.3) in a systematic manner, let us introduce 
a systematic procedure proposed by Pieper. 


It consists to start from the global transformation (5.4.2) and extract recursively from it the 
following equalities 


T =° A; +A, 2 As 3 A4 -4 A5 9 Ac =" Ag 


MATT qe 1452 As -3 A4 -4 A; -5 As =! Ag 
a5) PA- T= 2A; -3 A4 -4 A; 5 Ag =? Ag (5.10.4) 
tA DAS? CAT! T= =" As 


It is then obvious that for each equation of (5.10.4), 


e The left-hand side is a function of the task coordinates a (contained in the n, o, a and p 
vectors) and of the joint coordinates (q1, q2,..- qi, 41- 1,...5). 
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e The right-hand side is a function of the remaining coordinates (q;,1,...q6), unless some 
zeros or constants appear in the explicit expressions of matrices ^^! Ag. 
The trick of the method is to identify the elements of **1Ag which allow to extract an explicit 


expression for qi. 


5.10.3 General procedure to determine joint angles 


Before applying the technique to the kinematic model of the PUMA, let us note that 


ATAN2 function 


In order to determine the joint angles in the correct quadrant, they should always be evaluated 
from their sine and cosine values u and v by 


0 = ATAN? (u,v) (5.10.5) 


where ATAN2 (u,v) is the tan~! function modified in the following way 


tan 4 ifv>0 
0 = ATAN2 (u,v) = 4 tan ! * + ssign(u) ifv «0 (5.10.6) 
T $sign(u) ifv=0 


General solution of trigonometric equation a cos0 + bsin0 = c 


Equations of type 


acos@+ bsin@ =c (5.10.7) 
can be solved by assuming 
a= psing b = pcos ó (5.10.8) 
giving 
p=Va?+0? and $ = ATAN2 (a,b) (5.10.9) 
in which case 
c c 
sin(0 + 9) = F cos(0 + 9) = 4/1 — 2 (5.10.10) 


The solution to (5.10.7) is thus 


2 
0 = ATAN? (5, x ]1— 55) - ATAN2 (a,b) 
p p 


0 = ATAN? (c, /p? — c) — ATAN? (a,b) (5.10.11) 


It has 
e 2 solutions if p? = a? +b? > c? 
2 


e 1 solution if p? = c 


e 0 solution if p? < c. 
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5.10.4 Calculation of 0, 
According to Pieper's technique and making use of (5.4.1) and (5.4.8), letus express the equation 


Ci $1 0 0 | [ns Or Ax Pe | 


0 0 -1 0 Ny Oy Gà 

0A-1 T y Oy Gy Py 

Ar -S&S C, 0 0 Nz Og Qz Dz (5.10.12) 
0 0 0 1 0 0 O0 1 


in the form 


4-1. T fi(n) fix(o) fi2(@) fix(p) (5.10.13) 


with the functions 


fii (A) = CiAg + 81A 
fist) eed (5.10.14) 
fis(A) = —91A + CiAy 


where A is n, o, a or p. Let us identify it with 1Ag (where dg is set equal to 0 for the wrist center 
location) 


* k * So3d4 + à2 C^» 
1 u * * * —Co3d4 + a282 
Ag = DNE di (5.10.15) 
0 0 0 1 
One obtains directly 
fis(p) = —S1ps + Cip, = de (5.10.16) 
Equation (5.10.16) has the general form (5.10.7), and its solution is thus 
0; = ATAN2 (py, px) + ATAN2 (da, +,/p? + p2 — d2) (5.10.17) 


Equation (5.10.17) provides two possible values of 01 which correspond to right- and left-hand 
configurations (figure 5.10.1). Note that the multiplicity of the solution to 01 is generated by 
the arm offset d». 


5.10.5 Calculation of 0» and 0; 


Rather than following strictly Pieper's technique, it is easier to still use the information contained 
in equations (5.10.12)- (5.10.13). The following relationships may be extracted 


fu(p) = Cis + Sipy = S23d4 + a202 = Q 
fiz(p) = —pz = —Casda + a292 = B (5.10.18) 


Summing the squares gives 
o? + B? = a$ + då + 2a3d483 (5.10.19) 


and a second equation for $3 can be generated in the form 


aC» + BS2 = as + S3d4 (5.10.20) 
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are 


Figure 5.10.1: four possible solutions for the PUMA 560 arm 


Elimination of $3 between (5.10.19) and (5.10.20) provides an expression for 02 


o? + 8? + a2 — d2 = 


aC» + 855 = 2a 


(5.10.21) 


which according to (5.10.11) has the solution 


0; = —ATAN2 (a, B) + ATAN2 (y, d /o? + 82 — 42) (5.10.22) 


Again, two possible solutions appear which correspond to upper and lower arm configurations 
(see figure 5.10.1). 


Finally, 03 can be obtained by making use of (5.10.20) and of another equation for 03 gen- 
erated by 
a5 E 8C» = C3d4 (5.10.23) 
giving 
63 = ATAN2 (aC + 855 — a2,aS2 — 8C3) (5.10.24) 


Let us note that equations (5.10.17), (5.10.22) and (5.10.24) involve only position parameters: 
they correspond to the solution of the uncoupled position problem 


p = f (91, 62, 3) (5.10.25) 


5.10.6 Calculation of 0,, 05 and 0s 


'To obtain the values of wrist angles, we rely again on Pieper's technique by making use of the 
relationship 


SAL et Ase UAI UT EA, (5.10.26) 
which can be put in the form 
fau(n) fa(o) fa(a) fa(p)-a» * * C495 0 
faa(m) fs2(0) fs2(a) fs2(p) E * * S455 0 (5.10.27) 
fas(m) fs3(0) fs3(a)  fss(p)— do —S5Cg S596 Cs d4 iE 


0 0 0 1 0 0 0 1 
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with the functions 


fa(A) = C1 Co3A2 T S1C23Ay = So3A2 
fa2(A) = —S1Az + Cry (5.10.28) 
faa (X) E C1553A, + 5S S23Ay + C322 


with A representing n, o, a or p One finds directly the angle values 


04, = ATAN2 [fa2(a); fai(a)] 
05 = AT AN? [Cs fai(a) + S4 fax(a), fas(a)] (5.10.29) 
— ATAN2 [fa (0), — f33(n)| 


5.11 Numerical solution to inverse problem 


Let us suppose first for sake of simplicity that the geometric kinematic model can be put in the 
form 


zi = fi(q;) (@=1,...n, j=1,...m) (5.11.1) 
and that, for given z;, an approximate solution q7 is available. 


The principle of all iterative techniques to solve a nonlinear problem of general form (5.11.1) 
consists in calculating a correction óq; to the solution such that 


vi = fi(qj + qj) (5.11.2) 


A first order Taylor expansion of equation (5.11.2) provides the expression 


= fi(q7) DS ; + O(0q2) (5.11.3) 
Let us define next 
z; = fila) (5.11.4) 
and the residual quantity 
Ti = ti — T} (5.11.5) 


and let us observe that the right-hand side in (5.11.3) involves the jacobian matrix of the system 


J = H dimension nxm (5.11.6) 
qj 


Equation (5.11.3) can then be written in the matrix form 
r = Jóq + O(óq?) (5.11.7) 
which forms the basis for iterative solutions. 


The method of solution depends on whether m = m or not. Three cases have thus to be 
considered: 


5.11. NUMERICAL SOLUTION TO INVERSE PROBLEM 


Starting 
Procedure 


q =q 


Residual Vector 
“= x - fq) 


Convergence 
Check 
(^| «e 


Evaluation 
of correction 
r = JE sq‘ 


Correction 
q — q' 3 q“ 


Figure 5.11.1: Newton-Raphson solution to inverse problem 
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a)m=n 
Provided that the jacobian matrix remains non singular, the linearized equation 


r= Jôq (5.11.8) 


possesses then a unique solution, and the Newton-Raphson technique may then be used to solve 
equation (5.11.1). The principle is then as described by figure 5.11.1. 
Note that: 


- The cost of the procedure depends on the number of iterations to be performed, which depends 
itself upon parameters such as the distance between estimated and effective solutions and 
the condition number of the jacobian matrix at the solution. 


- Since the solution to the inverse problem is not unique, it may generate different configurations 
according to the choice of the estimated solution. 


- No convergence may be observed if the initial estimate of the solution falls outside the 
convergence domain of the algorithm. 


Much effort is thus needed to develop more robust numerical solutions to (5.11.1). 


b)m<n 


This is the overdetermined case for which no solution exists in general, since the number of 
joints is not sufficient to generate an arbitrary configuration of the tool. 


A solution can however be generated which minimizes the position error. Consider the 
problem 


mu fr- Deb ser (5.11.9) 
or, in matrix form 
pu fr- Te- fa Wie - fiq)] (5.11.10) 
where 
W = diag(w; ... wn) (5.11.11) 


is a set of weighting factors giving a relative importance to each of the kinematic equations. 


The position error is minimum when 


DES E T A E 
T =D gg tb — fin -0 (5.11.12) 
or, in matrix form 
J^W|z — f(q)] - 0 (5.11.13) 


A Taylor expansion of the third factor shows that the linear correction to an estimated solution 
q“ is 
JTW |æ — f(q*) — Jóq] = 0 (5.11.14) 
The correction equation is thus 
J'WJóq = JTW r (5.11.15) 
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where r is the residual vector defined by (5.11.5). 


Since W is a positive definite diagonal matrix, the matrix JTW J is always symmetric and 
may thus be inverted. It provides the generalized inverse to the jacobian matrix 


Jt -(JTWJ) Jw (5.11.16) 
which verifies the property 
JUI (5.11.17) 


It is easy to verify that whenever J is invertible, 
Jr (5.11.18) 


and the solution to (5.11.10) does not differ from the solution to the nonlinear system (5.11.1). 


c) m»n 
'This is the redundant case for which an infinity of solutions is generally available. Selection of 


an appropriate solution can be made under the condition that it is optimal in some sense. 


For example, let us seek for a solution to (5.11.1) which minimizes the deviation from a 
given reference configuration qo. The problem may then be formulated as that of finding the 
minimum of a constrained function 


A 1 
a fr- gia - aol" Wia - a} (5.11.19) 
subject to 


z— f(q) =0 (5.11.20) 


Using the technique of Lagrangian multipliers, problem (5.11.19)-(5.11.20) may be replaced by 
the equivalent one 


oG oG 


DS — = .11.21 
Dg 0 Dr 0 (5 ) 
with the definition of the augmented functional 
1 
G(a. à) = 5[a — ao)" Wa — ao] + à” [æ — f()) (5.11.22) 


It leads to a system of m + n equations with m + n unknowns 


Wía - q] - 74 —0 
x — f(q) =0 (5.11.23) 


Linearization of equations (5.11.23) provides the system of equations for the displacement cor- 
rections and variations of lagrangian multipliers 


Wôq — JTôA =0 
Joq=r (5.11.24) 
Substitution of the solution óq obtained from the first equation (5.11.24) into the second one 


yields to 
JW !J'óA-r-m (5.11.25) 
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x = 0 
(T) 
Q) 
Figure 5.12.1: Linear mapping diagram of jacobian matrix 

or, in terms of the displacement correction 

ôq = W JT (JW JT) tr (5.11.26) 
The matrix 

J'-wWw UUweg- (5.11.27) 


has the meaning of a pseudo-inverse to the singular jacobian matrix J. It verifies the identity 
JI =I (5.11.28) 


and again, whenever J is invertible, 
J =J (5.11.29) 


5.12 Linear mapping diagram of the jacobian matrix 


At the light of the results of the previous section, it is interesting to interpret the properties of 
the jacobian matrix in a linear algebra context. 


The jacobian matrix J relates the infinitesimal displacements óz = [6x1 ...6a,] of the end- 
effector to the infinitesimal joint displacements óq = [dq1 ...óq,,] and has thus dimension n x m. 
When m is larger than n and J has full rank, there are m — n redundancies in the system to 
which correspond m — n arbitrary variables. 


The jacobian matrix J determines also the relationship between end-effector velocities « and 
joint velocities 
t= Jq (5.12.1) 


Equation (5.12.1) can be regarded as a linear mapping from a n— dimensional vector space A" 
to a m—dimensional vector space Q™ which can be interpreted as displayed in figure 5.12.1. 

The subspace R(J) is the range space of the linear mapping, and represents all the possible 
end-effector velocities that can be generated by the n joints in the current configuration. If 
J has full row-rank, which means that the system does not present any singularity in that 
configuration, then the range space R(J) covers the entire vector space X". Otherwise, there 
exists at least one direction in which the end-effector cannot be moved. 


The null space A/(J) of figure 5.12.1 represents the solutions of Jq = 0. Therefore, any 
vector d € N(J) does not generate any motion of the end effector. 
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If the manipulator has full rank, the dimension of the null space is then equal to the number 

n — m of redundant DOF. When J is degenerate, the dimension of R(J) decreases and at 

the same time the dimension of the null space increases by the same amount. Therefore, the 
relationship holds 

dim R(J) + dim N(J) =m (5.12.2) 


Remark 


Configurations in which the jacobian has no longer full rank corresponds to singularities of the 
mechanism, which are generally of two types: 


- Workspace boundary singularities are those occurring when the manipulator is fully stretched 
out or folded back on itself, in which case the end effector is near or at the workspace 
boundary. 


- Workspace interior singularities are those occurring away from the boundary, generally when 
two or more axes line up. 


5.13 Effective computation of Jacobian matrix 


According to the results of section (4.16), the infinitesimal transformation matrix 


ôR or 
ôT = | or o0 | (5.13.1) 
gives rise to the differential matrix 
T-T! = | P | (5.13.2) 
where 
0 —daz,  óo, 
óà —óR.RT— | ða, 0 dae (5.13.3) 


—óoy Qr 0 


is the matrix of infinitesimal rotations, and dv is a vector related to infinitesimal displacements 
such that 
ôr = dar + ôv (5.13.4) 


Collecting thus the information contained in (5.13.2), one obtains the variation of vector x 
describing the end-effector configuration in task space 


bx = | is | (5.13.5) 
One knows on the other hand that T is a function of joint coordinates (q1, ...@m) such that 
T = Aı (qı) - A2(q2)--- Am(Gm) (5.13.6) 
Its differentiation leads thus to 
= AiG) Ae Anam (5.13.7) 


Odi 


i=1 
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Let us next express the partial derivatives in the form 


ðA 
Odi B 


AA; (5.13.8) 


By referring to the DH transformation matrix (5.2.2), it is easy to verify that 


e for a revolute joint: 


As | x i | (5.13.9) 
since it has the meaning of a unit rotation about z axis, and 
e for a prismatic joint: 
Aye | v d | (5.13.10) 
0^ 0 
since it corresponds to a unit displacement about the z axis. 
Let us next express each term of the series (5.13.7) in the form 
Ai(qi) As). ...Am(dm) = Ci T (5.13.11) 
where C; has the expression 
C; = (A1: Ao. ACUTE (Ag AAT) 
= (Ai: Ao... Aj. 1) Ai(Ai: Ag... Aj_1)7* (5.13.12) 
and is thus of the form 
C; = | E a | (5.13.13) 


Alike equation (5.13.2), it contains six independent terms which we can combine in the vector 


a=] E | (5.13.14) 


Vi + Gir 


The jacobian matrix describing the instantaneous kinematics in configuration T' may then be 
obtained from the relationship 


da = V ^ dióqi (5.13.15) 
w=1 


In terms of the individual matrices (5.13.14), its full expression is 


m Ci C2 EP Cm 
J= | Vi +#Čır voter ... Um + ČmrT (5.13.16) 


In order to interpret geometrically the result (5.13.16), let us consider separately the case of 
revolute and prismatic joints. 
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Figure 5.13.1: Geometric meaning of revolute and prismatic joint contributions to jacobian 
matrix 

e For a revolute joint, A; is given by equation (5.13.9) and C; may be written in the form 

C; A1 A; AL (5.13.17) 

If R; , is the rotation operator from reference frame to frame attached to link i — 1, then 

či = Rii- k. RI, (5.13.18) 

which means that the direction c; is the direction of joint axis i in global coordinates, and 

vi = —Čifi—1 (5.13.19) 


The contribution to J of a revolute joint is thus 


di = | &(r om | (5.13.20) 


where the translation part represents the cross-product of the rotation direction times the 
relative position of the effector in frame i — 1. 


e For a prismatic joint, equation (5.13.10) does not generate any rotation contribution, and 
Vi = Rj jk = Cj (5.13.21) 


is the direction of the joint axis in global coordinates. Therefore, 


a=] a | (5.13.22) 


Ci 


5.14 Jacobian matrix of Puma manipulator 


The Jacobian matrix J relates the linear and angular velocities of the hand system to the 
velocities of the joints. 


H E E tonta ai E E (5.14.1) 
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Because PUMA robot is 6 dof robot, the Jacobian matrix is a 6 x 6 matrix whose ith column 
vector J;(q) is given by: 


im if joint i is rotational, 
Zi-1 X D(i—1),6 
Ji(q) — (5.14.2) 


o E T x 
| | if joint i is translational. 
z 


The vector q(t) is the joint velocity vector of the manipulator while p(; 1j; is the position of 
the hand coordinate frame from the (i-1)th coordinate frame expressed in the base coordinate 
system and z;_, is the unit vector along the axis of motion of the joint i expressed in the base 
frame as well. 

For the Puma manipulator which is a 6R type robot, we have: 

J(0) = a = P 78 is xm (5.14.3) 

Zo X poo 71% puo 22X P26 Z3 X P36 Z4*X pao %5 X P56 

For the PUMA, we use the results of direct kimenatic model to determine the different 

column vectors of the Jacobian matrix. 


Joint 1 


The contribution of joint 1 is quite easy to determine from °Ag. The direction of joint axis 1 is 
given by zo. As coordinate 0 is the reference frame, we have: 


0 
zo = k= 0 
1 
The translation part is given by: 
Zo X P0,6 
with po,s which is the translation part of °Ag. 
C; [C53 C4S5dg + S23(d4 + Csdg) + a202] — S1(S495dg + d2) (pe) « 
"ps = | S1[C23CaSsdg + S23 (da + Csde) + a2C2] + Ci (SaSsd6 + d») | = po), 
(— S230455 + C23C5)dg + Co3d4 — a5» (9po) z 
It comes: 
. 0 —1 0] [(9po); — (8p), 
kx "pa—k'pg— 1 0 0} |(pse),| = | Cpo)« 
0 0 0| | (pe). 0 
Therefore we have: 
0 
0 
1 
Ji(0) = (5.14.4) 


— $1[C23C4S5ds + $23(d4 + Csdg) + a2C2] — C1(S4S5dg + d2) 
C1 [C23C4S5de + S23(da + Cade) + a3C2] — $1(S4S5d6 + d2) 
0 
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Joint 2 


Determining the orientation of joint vector 2, i.e. axis z1, requires the rotation of body system 
1 with respect to base system °R; which is given by the rotation part of A4. Thus we calculate 
the absolute direction of the revolute joint axis: 


0 Ci 0 —94 0 —6$1 
Z1 = R; 0 = Si 0 Cı 0 = Ci 
1 0 -1 0 1 0 


For the translation component z4 x pi,6, we need the position of the hand system with respect to 
body system 1: pie. This vector pı, can be extracted from the translation part of ‘Ag, but tpe 
is expressed in local coordinates of system 1: O1,21,y1, 21. Then we have two possibilities. The 
first one is to express the two vectors in the reference coordinate system 0 (i.e. pig = OR, !pe) 
and then calculate the cross product zı x pis. The second alternative, which is generally the 
easier one) is to determine the product in local frame 1 (i.e. k x tpe) and then express the result 
in the base frame with rotation matrix °R,. The second method is chosen. It comes: 


zi X pi,6 = °Ri(k x !po) 


Cos C4S5dg + So3(d4 + Csdg) + a3C5 
Ipe = | $23C495de — C23(d4 + Csdg) + a2S2 


S4S5de6 + d2 
Cı 0 —S$1] | —(S23C4S5d6 — C23 (da + C5de) + a282) 
zı X P1,6 = $1 0 C1 C53 C45S5dg + S53 (da + Csdg) + 7107 
0 -—1 0 0 


—Ci[S23C4S5de — C23(da + Csde) + a282] 
= |-—S1[S23C4S5ds — C23(d4 + Csdg) + a282] 
— [C»3 C4 $5dg t S23(d4 t Csdg) t aC] 


Therefore, J2 is given by: 


mt 
C; 
ams ? (5.14.5) 
—C1[923C4S5de — C»a(d4 + Cade) + a282] 
—S1|[923C4S5de — C23(da + C5dg) + a282] 
— [C»3 C4 $5dg t S23(d4 t Csde) t aC] 
Joint 3 
For joint 3, the procedure is the same, but we have to calculate "Ry =° R; Re at first. 
OR, = "R tR 
Cy 0 =S IC -S2 0 CCo —C,4S, —Sı 
= S 0 C1 Sp Co 0| = |$1€, —S19, Ch 
0 -l 0 0 0 1 — S2 —C5 0 


22 = Rə k = Ci 
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The translational contribution is given by: 


z2 X p26 = Ra(k?pg) 


We have 
C3C4S5dg + S3(d4 + Csds) 
?ps = |S3C4S5de — C3(da + Code) 
S4595dg 
—[93C4S5ds — Cs(d4 + Csds)] 
kx ?p6 = C3C4S5d6 + S3(d4 + Csdg) 
0 


63 Cede = Ca (da + Csdo)] 
C3C485ds + S3(d4 + Csds) 
0 


C1C; -Ci1$9 -Sı 
S10; —9199 Cı 
— 99 —C»5 0 
—Ci[$53C4S5dg — Cos(d4 + Csds)] 
—S1[S23C41S5dg — Cos(da4 + Csda)] 
C»3C4S5dg — S23(da + Csdg)) 


Z2 X P26 = 


And Jz is given by: 
-5S 
Cı 
0 
J3(0) = .14. 
3(9) —C4[$53C4 $5ds — Co3(da + Csds)] TES) 
—S,[S53C4S5de — C23(d4 + Csdg)) 
C53C4S5dg — S23(d4 + Csds)) 


Joint 4 


Contribution of joint 4 to Jacobian matrix is: 


s [ate 


Z3 X pa,6 


At first we calculate the direction of the joint axis z3. It comes: 


OR, = ?R;?R; 
CC» —C1 S2 — Sı C3 0 S3 C1 C23 — Sı C1 S23 
PE S1C2 — S1 S2 Cı S3 0 —C3 = 51 C23 Ci $1553 
— $5 — C5 0 0 1 0 — S23 0 C23 


The joint axis direction in the base frame is then 


C1 S23 
51553 


23 =? R3 ks 
Cos 
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'The translational contribution is given by: 


— C4S85dg 
Z3 X pao =  "Ra(kx “ps) = "Rs | S495de 
0 
C1Co3 —541 C1 S23] |—5455de 
= |S810 Cy $1553 Ca455dg 
— S23 0 C53 0 
—C1C535455dg — 51C455ds 
= |—S1C23848S5de + C1C455de 
5235455ds 
Finally contribution of joint 4 is given by: 
| C1 S23 ] 
51553 
C23 
0 = 
Jae esteso odo ser side 
—51C535455ds + C1C455ds 
5235455ds 
Joint 5 
Joint 5 contributes to Jacobian matrix with: 
J= p 74 | 
4 X p46 
The direction of the joint axis 5 is given by z4. It comes: 
OR, = ORs 3R4 
CiCog —91 C1S23) [C4 0 -—94 
= 1910 Ci 5S$1553||$, 0 C4 
— S23 0 C23 0 -—1 0 


I 


—S53C4 C23 $5394 
and the direction of joint axis 5 in the base frame is then 
—C1C5354 — $1C4 
5$1C5354 + C1C4 
S2354 
The translational contribution of the joint is given by: 
—Csds 
Ssde 
0 


ZA =° R; k= 


z4 X P46 = ?R4(k x pe) = OR, 


—$o3C4 Coa S354 
(—C1C23C5 — $1$5)de 
(—S1Co3Cs5 + C155)dg 

$23 C5de 


| 


C1 Co3C4 — S184 —C1993 —C1C23S4 — $1C4 
S1C53C4 + C454. —S1S23 $1C5354 + CC, 


CiCo93C4 — S184 —C1S23 — C1 C2384 — $1C4 
S$1C593C4 + C484. —91593  S1C535$4 + CiC4 
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38 CHAPTER 5. KINEMATICS OF SIMPLY CONNECTED OPEN-TREE STRUCTURES 


Finally contribution of joint 5 is given by: 


—C1C5354 — $1C4 
S1C5394 + C1C4 
$2354 
(—C1C23C5 — $455)dc 
(—S1C23C5 + C155)dc 
823C5dg 


Js(0) = (5.14.8) 


Joint 6 


The 6th column of the Jacobian matrix is: 


Jy = jo 
Z5 X P5,6 


We calculate at first the direction of the joint axis 6: 


OR. = RAR 
C1Co3C4 — S184 —C19»3  —Ci1C53584 — 91C4 | [C5 0 Ss 
S1C53C4-- C494. —915»3 $1C23S4 + C1C4 S5 0 —Cs5 
—S93C4 C23 $2354 0 1 0 
[C1 Cos C4 ud $194]C5 = C1 $2355 — C4 C53 S4 = SiC, [(C1C23C4 = S1S4]S5 SE: C15S53C5 
[$1C23 C4 + C1S4]C5 + S182355 S1CosS4 +C1C4  [S1C23C4 + C184]S5 + S1823C5 
—S23C4C5 — C2355 S23 94 —S23C455 + Co3C5 


Il 


Il 


and the direction of joint axis 6 in the base frame is then 


C1 C53C485 — 819495 + C15923C5 
25 =? Rs k= S1C23C4S5 + C4 S455 zs S182305 
—S23C4 55 + Cos C5 


The translational contribution of the joint is given by: 
z5 X ps6 = °Rs(k x "pg) = o 
Finally contribution of joint 5 is given by: 


C1C23C485 — 8154895 + C1953C5 
S1C53C485 + C1845855 + S182305 
Je(0) = B TRASS (5.14.9) 
0 


0 


5.15 Effective numerical solution of inverse problem 
Let us consider the problem of finding (q1, ...4,,) such that 


starting from an approximate solution (qf, ...97,). 
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First, let us construct the matrix T* corresponding to the given configuration 
T* = Ai(qi) : A2(q3) ---. Am (a5) (5.15.2) 
'The residual error on the transformation matrix can be defined as 
ôT =T — T” (5.15.3) 


and a first order expansion of equation (5.15.3) provides the corresponding expression in con- 
figuration space 


ôT = Y (£163) -T + O(óq?) (5.15.4) 


i=1 
with the definition (5.13.12) of C; matrices. 


To the first order, one can thus obtain the joint displacement corrections from the equality 


ôT- T! = Y XC;óg) (5.15.5) 


i=1 
From the left-hand side, six independent infinitesimal quantities can be extracted 


daz = (ÔT -T*)32 ~ -(0T - T*)as 
day = (6T-T sc -(T-T )a (5.15.6) 
óa = (ôT T!) m —(ôT -'T-3)is 


ór, =(6T-T) yy — Óóozr, + óogrz 
óry = (ôT g T sg + 6Azlx — OAnT x (5.15.7) 
ór, =(6T-T~*)34 — bye + óagr, 


in which case 
bat = [daz ay daz Ory dry Orz] (5.15.8) 


and the correction equation may then be written in the matrix form 
Jq = ôx (5.15.9) 
Equation (5.15.9) is then directly invertible if n = 6 and J has maximum rank. 


The numerical procedure of figure (5.11.1) remains valid, except that the residual vector has 
to be evaluated from equations (5.15.6)- (5.15.7). 


Remark 


It is important noticing that vector ĝa, since it is defined in a finite rotation context, is an 
non integrable quantity. In a classical mechanics sense, a has the meaning of a vector of quasi- 
coordinates and exists only through its variation da = (da, da, óo;) which represents angular 
increments in the instantaneous configuration. 
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5.16 Recursive calculation of velocities in absolute coor- 
dinates 


Our goal is to take advantage of the open-tree architecture described by equation (5.13.6) to 
compute 


A=T.T" (5.16.1) 


To this purpose, let us start from the transformation for frame i 
T; =° A; =° A; -t Ag... 1A; (5.16.2) 


describing the configuration of link ¿ into the global frame, and let us define the differential 
matrix describing its absolute velocities 


oÁ; = $,- Tt (5.16.3) 
Then, the transformation for link i + 1 is 
Tii = T; į Aigi (5.16.4) 


and 


0 Ai = D T 
-( Ain +È; AG) TU (5.16.5) 


It provides the recursive formula 


Ani = Å +T ÅT! (5.16.6) 
whose meaning, according to the type of joint considered, is the following: 


e For a revolute joint, the differential matrix in local coordinates is 


l } k 0 
Aii = dini | oT 0 | (5.16.7) 
and we have thus 
Wit. = Wi + Gi4t1Ci41 (5.16.8) 
and 
Vig = Vi — G4 Cigili (5.16.9) 


from which one deduces the translational velocity 
Tiri = Ti + Oi41(Ti41 = Ti) (5.16.10) 


Equation (5.16.8) means that angular velocity of frame i+ 1 is the angular velocity of 
frame i augmented by the relative angular velocity produced by joint q+1. Its linear 
velocity (5.16.10) is obtained by adding to the linear velocity of frame i the contribution 
of the rotation of link i 4- 1. 
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e For a prismatic joint, the differential matrix in local coordinates is 


i : 0 k 
Aia = dia | oT o | (5.16.11) 
and we have thus 
Wi+1 = Wi 
Tig. = fi + Dini (Ti41 ae ri) + Gi41Ci41 (5.16.12) 


Angular velocity of frame i + 1 is the same as that of link i, and linear velocity contains 
two contributions: the relative velocity due to angular motion of link ¿+ 1 and the relative 
velocity due to linear motion at joint q;. 


5.17 Recursive calculation of accelerations in absolute co- 
ordinates 


To compute accelerations recursively, let us similarly compute 
A-2T.T (5.17.1) 


by defining at an intermediate stage the differential matrix describing the absolute accelerations 
of link i . 7 
OA =T;-T,' (5.17.2) 


Then, the acceleration matrix for link i + 1 is 


A Q1 = Tis: Tai 
= (T; ê Åi + 2T; tÅ +T, S AGAT, 


a 


SÈT -TÓAG4ADOLnUPTTUILUABWGE 


a 


and can be put in the recursive form 


[Any = VA; 4+2°A;-T,? Agi T! +T, i Agi UT. (5.17.3) 


In order to interpret it, let us consider successively 
e The revolute joint: 


The differential matrices in local coordinates are 


iA i k 0 
Ain = iyi | oT 0 | (5.17.4) 
and " een 
ie " k 0 . kk? 0 
Aia = qi} | oT 0 | = 4h | oT o0 | (5.17.5) 


For angular accelerations, we have thus the recurrence relationship 


! m E E = S» de ~T 
Bisa = Bi + Gi Origa + ği+1Či+1 — G4 Či+1Či}1 
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with 
— £5 Quot d Ns ~ oT 
Bi = Ŭi — ið; an Big = Gigi — D410) 44 


Therefore 
Wig = Wi + (Wi Wy — GWT) + 24:41 Wiig + GiGi — 41 Čir čla (5.17.6) 
Making next use of (5.16.7) we get 
Oi Oy — Uu = din (Čiča 25 6 4911) T 6a 
If we note further that 
2ðiČi+1 + Gh + Čip = WiCi41 — C4105 = WiC) — cau. 
we obtain the final result 
Wit = Wi + diqy1QiCiq1 + ği+1Ci+1 (5.17.7) 
Similarly, for the linear part of (5.17.3) by performing the triple products we get the result 
Gina = ai — (26410; + diui — G7. Gig Gigi: 
which, by taking account of 
and (5.17.7), can be put in the final form 
Pipi = Pi + (ipi — O41 O2,4)(rin — ri) (5.17.8) 
e The prismatic joint: 
The differential matrices in local coordinates are 
fAui2dui | T : | and  'Áia4— qiu | = : | (5.17.9) 
and equation (5.17.3) provides thus the relationships 


Bii = Bi (5.17.10) 


and 
Vier = Vit Ci41 Gigi + 20; Cici1dis1 (5.17.11) 


From (5.17.10), we deduce that the angular acceleration remains unchanged 
Gil = Wi (5.17.12) 
while 


Pipi = Pi + (Di — Gi?) (rig — ri) + ciadaa + 20;ciidaa (5.17.13) 


To summarize: 
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1. The differential matrices verify the relationships 


0 À, PÀ; +T, ê Ap Tt 
0A LÅ; + LÀ; T; Å Åi T HT, i Ai E (5.17.14) 


2. For a revolute joint 
Girl = Wi + di+1Ci+1 
Piti = Pi + ipi (ripi — ri) 


Gigi = Wi + ği+1Ci+1 + G41 Oici41 


Tiii = Ti e (Gia = Dizia) (ri > ri) (5.17.15) 
3. For a prismatic joint 


Wi+1 — Wi 
Pigi — Pi + Oiya (Tipi — Ti) + diacia 


Wi+1 = Wi 


Pipi = Pi + (Gui — Diy) (ri — Tri) + Ci+iği+ı + 290i 41Cididi4i (5.17.16) 
The forward recursive kinematic equations (5.17.14) to (5.17.16) will form the basis for an 


efficient formulation of dynamic equations, either in Lagrange or in Euler-Newton form. 


5.18 Recursive calculation of velocities in body coordi- 
nates 


Exactly in the same way as we have computed A in section 5.15, let us consider the matrix 
=T. T (5.18.1) 


It is easy to verify that it has the meaning 
g » 1 
P= | or 6 | (5.18.2) 


where . 
à = RTR (5.18.3) 
is the matrix of angular velocities expressed in body coordinates, and 


v = Rr (5.18.4) 


represents the linear velocities expressed in the same frame. Let us start again from the differ- 
ential matrix describing the velocities of frame i 


BET, (5.18.5) 
Since the transformation from frame i+ 1 is 


Ti = T; Ai (5.18.6) 
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then 


D41 = T (T; “Aina +T; Aia) 


and it provides the result similar to (5.16.5) 


A ae ee ae 
Ha = "AG Ti Aig Aga Aig 


or 


Lao = AQ) i +! Aig] Ai (5.18.7) 


Let us define: 
- R44 the rotation operator from frame i to frame i + 1 


- c a parameter characterizing the type of joint: c = 0 for a revolute joint, c = 1 for a prismatic 
joint 


- d*,, the vector expressing the position of the origin of frame i + 1 in frame i (displacement 
part of *A;,1). 


- wx, the angular velocity of frame i + 1 expressed in frame i 


The angular velocity of frame i + 1 expressed in frame i is given by 
Uia =w,+(1—o)digik (5.18.8) 
Then, velocity propagation in frame 7+ 1 can be expressed in the form 


/ — T * 
Wi, = Ri 1Wi+1 


vi, = Rl lv; + Gta dta + okġi+1] (5.18.9) 


5.19 Recursive calculation of accelerations in body coor- 
dinates 


To compute accelerations in body coordinates, we similarly consider the matrix 


r=T'.T (5.19.1) 


for which it is easy to verify that it has the meaning 


.. 4 4 
f= | i is | (5.19.2) 
where 
a = RF (5.19.3) 


is the acceleration vector in body coordinates, while the angular acceleration w’ can be extracted 
from the form E 
B' = RTR = č -(oyo (5.19.4) 


Starting from : 7 
I, =T;'.-T, (5.19.5) 
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we obtain 


La- T,i(T, “Agi + 2T; “Angi T; Aja) 


which can be put in the recurrence form 


Fa = AN (4 20; i Aig H Aig] ‘Avs 


4-1 


It is easy to check that it provides the following recurrence relationships 


e The angular acceleration of frame i + 1 can be expressed in frame i by 
Gia =O; + (1-0) [Gi418; + Gti) 
e The acceleration propagation may then be expressed in the form 


M EN T ex 
Wit. = Riwi 


a = R} la; ar (Of, s (554) ^95,4)d24] t o(2di19;,4 + dii)k 


(5.19.6) 


(5.19.7) 


(5.19.8) 
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Chapter 6 


DYNAMICS OF OPEN-TREE 
SIMPLY-CONNECTED 
STRUCTURES 
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x(t) a(t) 
(t INVERSE ó(t) INVERSE Tt) 
X(t) KINEMATICS 6(t) DYNAMICS 


Figure 6.1.1: The inverse dynamic problem 


6.1 Introduction 


The dynamic equations of motion of an articulated structure describe its dynamic behavior. 

They can be used for several purposes: 

e computer simulation of the robot arm motion, 

e design of suitable control equations, 

e evaluation of the dynamic performance of the design. 

Just as in kinematics, the problem of robot arm dynamics may be considered from two 
complementary point-of-views. 

e The inverse dynamics problem; 


e The direct dynamics problem. 


The inverse dynamics problem 


The inverse dynamics problem consists, given positions, velocities and accelerations, to com- 
pute the forces and torques necessary to generated the prescribed trajectory. It may thus be 
summarized as presented in figure 6.1.1. 


For control purpose, the inverse dynamic model is used to determine the nominal torque to 
achieve the goal trajectory : the controller is then left with the responsability to compensate 
for deviations from the nominal trajectory. 


The inverse dynamic model is also used at design level to evaluate the power requirements 
at actuator level. 
The direct dynamics problem 


The direct dynamics problem consists, given an initial state of the system (specified by initial 
positions and velocities 69 and 69), to predict the trajectory when the torques are specified at 
the joints. 


The dynamic model takes the form of a system of differential equations in time, with coefficients 
which are configuration dependent 


M(0)6 + A(0) 6? + G(0) = c(t) (6.1.1) 
where 
- M (0) is the inertia matrix of the system, 


- A(0) 8? represents centrifugal and Coriolis terms, 
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e(t) x(t) 
2090 DIRECT 6(t) DIRECT x(t) 
DYNAMICS 6(t) KINEMATICS x(t) 


M 


(0) 6(0) 


Figure 6.1.2: The direct dynamic problem 


- G(0) represents the action of gravity on the system, 


- T(t) represents the external torques applied on the system. 


When considered in inverse form, the dynamic problem consists simply in algebraic calcula- 
tions, since trajectory information can be introduced in equation (6.1.1) to provide as an output 
torque values 7(t). 


The direct problem (Figure 6.1.2) is much more difficult to solve since it consists of a set of 
second-order differential equations with non-linear coefficients. Its use is mainly for computer 
simulation of the dynamics of the system. 


Evaluating the coefficients of the dynamic model (6.1.1) involves a significant amount of float- 
ing point operations. In order to minimize the associated cost, it is essential to take account of 
the recursive property of open-tree articulated structures. The problem is still further simplified 
when the structure is simply-connected, as it occurs in most industrial robot architectures. 


Before establishing the dynamic model of an articulated chain, we will establish the dynamic 
equilibrium equation of a single link, treated as an isolated rigid body. They will be expressed in 
matrix form, using either a body frame or a global frame representation. 


We will next build the dynamic model of articulated chain using a recursive Euler-Newton 
formalism: in the same way as a forward recursion propagates kinematic information, a back- 
ward recursion propagates the forces and moments from the end effector to the base of the 
manipulator. 


In the last part of the chapter, the same recursion concept will be used to establish the 
equations of motion in Lagrange form. 


6.2 Equation of motion of the rigid body 


Let us consider a rigid body of volume V (Figure 6.2.1) with its center of mass taken as the 
IM ul 


origin of the body frame O'z'y'z'. 


We suppose that it is submitted to a gravity field g, and that a force per unit of volume f is 
acting on it. 


Let us adopt Hamilton's principle to describe the dynamics of the system above. 
We define: 
- K : the kinetic energy of the body, 


- U : its potential energy (here, due to gravity), 
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X 


Figure 6.2.1: Kinematics of rigid body 


- OW : the virtual work of the non-conservative forces acting on the system. 


Hamilton’s principle states that, between two times tı and tz at which the position of the 
system is prescribed, the real trajectory of the system is such that 


t2 t2 
sf (K — Uu) dt + f SW dt = 0 (6.2.1) 
ty tı 


6.2.1 Potential energy 


Since the only form of potential energy considered is gravity, it can be expressed in the form 


uU = J g” (r + Rp') dm (6.2.2) 
V 
and, since O’ is the center of mass 
p'dm — 0 (6.2.3) 
V 
and equation (6.2.2) reduces thus to 
U = mgr (6.2.4) 


where m is the total mass of the body. 


6.2.2 Kinetic energy 


The kinetic energy is evaluated by integrating the kinetic energy of individual particles over the 
volume 


1 
k= >| v? v dm (6.2.5) 
2 Jv 
where the velocity at a given point is calculated by 
v = $ + Rp (6.2.6) 


When substituting (6.2.6) into (6.2.5), the kinetic energy of the rigid body can be decomposed 
into three terms 
K = Ki + Ko + K3 (6.2.7) 
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K is the translational kinetic energy 


Kı = 5 f 575 dm 
2 Jy 


It can be expressed either in global or body coordinates 


with 
v = Rt 


defined as the velocity of O' expressed in body components. 


Ke is the mutual kinetic energy 
K2 = PUR dm 
V 
and it vanishes according to (6.2.3). 


K2 = PRU | pf dm = 0 
V 
Ks is the rotational kinetic energy 
1 IT HT pou 
K3 = =- | p R Rp dm 
2 Jy 
It can be transformed by noticing that 
RTR = RTRRTR = () 
where 
a = R'R 
is the angular velocity matrix expressed in body axes. 


Let us next make use of 


~ EN] I 
wp = -pw 


to put equation (6.2.12) in the final form 


Ks ET Sy Su! 


where 
J’ = [wore dm = ] ^ 1- v9) dm 
V V 


is the tensor of the rigid body in the body axes. Its explicit expression is: 


(6.2.8) 


(6.2.9) 


(6.2.10) 


(6.2.11) 


(6.2.12) 


(6.2.13) 


(6.2.14) 


(6.2.15) 


(6.2.16) 


(6.2.17) 


(6.2.18) 
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Figure 6.2.2: Dynamics of a isolated link 


'The rotational kinetic energy can similarly be expressed in the absolute reference frame in the 
form 


K = Su Ju (6.2.19) 
where w is the angular velocity in the global axes 
w = Rw (6.2.20) 
and J is the inertia tensor in global axes 
J = RJ' R” (6.2.21) 
Note that it is time-dependent and that its time derivative can be expressed in the form 
Jj = J — Jð (6.2.22) 


According to (6.2.9), (6.2.11), (6.2.16) and (6.2.19), the kinetic energy (6.2.7) can thus be 
expressed in one of the alternate forms 


1 1 
K = Qm? + xv Jw 
1 Da 1 iT a 
= ge v + 3" Jw (6.2.23) 


6.2.3 Virtual work of external loads 


Supposing a force distribution f over the body, the global axis expression of the associated 
virtual work is 


ôW = / f' ó(r + Rp’) dV (6.2.24) 
V 
It can be explicited by making use of the fact that 


ó(r + Rp’) = ôr + 6a (p — r) (6.2.25) 
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where ĝa is the skew-symetric matrix 
óà = ôR RT 


Let us make next use of the fact that 


da(p—r) = —(p—r)da 
to put (6.2.24) in the final form 
OW = Fôr + N" da 


The vector 


F = i f dv 
V 
is the total force acting on the body, and 


n= | rf av 
V 
is the resulting torque about the center of mass. 
Equation (6.2.24) can still be expressed in body axes 
W = F” Sr! + Nôd 


where 
ór' = R” ôr 


is the virtual displacement of the center of mass projected into body axes. 


F' = R'F 
is the resultant force at the center of mass projected into body axes. 
6a’ = R'óR 


is the matrix of infinitesimal rotations, and 


N = ] Bien) RP Rf av 
V 


/ pf aV 
V 


is the torque about the center of mass. 


(6.2.27) 


(6.2.28) 


(6.2.29) 


(6.2.30) 


(6.2.31) 


(6.2.32) 


(6.2.33) 


(6.2.34) 


(6.2.35) 


6.2.4 Relationship between angular velocities and quasi-coordinates 


In order to express the variation of (6.2.1) where dw and da appear as separate quantities, it is 
necessary to recognize that the o parameters are quasi-coordinates in the sense that they are 
defined only in a differential manner. We need thus to express the relationship existing between 


da and dw. 


To get it, let us start from the definition 


óà = óR RT and © = RR" 
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we obtain on one hand 
da = RR? + RRT (6.2.36) 


and on the other hand 


6@ = RRT + RóRT (6.2.37) 
Equations (6.2.36) and (6.2.37) can be combined in the form 


60 = óà ôR RT + RRT (6.2.38) 
or 
60 = óà + 640-056 (6.2.39) 
If we note that 
640-06 = wóot —daw? = —(H 6a) (6.2.40) 


generates a skew-symetric matrix with components 


Wz ÔQy — Wy Oa, 
—(@ da) = |we daz — wz 6Ay 
Wy Oy — Wz dy 


we obtain that the associated vectors are related by 


bw = da — wda (6.2.41) 
Similarly, starting from 
6a’ = RTóR and w = RIR (6.2.42) 
we would obtain 
6a" = ôA +a 6a! —óoc (6.2.43) 


This can be further written as 


6a’ = óà + (Goa!) 


and this gives the result: 
bw’ = 6a’ + dal (6.2.44) 


6.2.5 Equations of motion 


In order to express the dynamic equilibrium of the rigid body, let us start first with global 
coordinates where the variation of the kinetic energy is 


1 
ôK = miT br + wh J dw + zt Mu 
The tensor of inertia J being configuration dependent, its variation can be obtained in the form 
ôJ = óàJ — Jóà (6.2.45) 


and owing to (6.2.41) and (6.2.45), 6K can be written in the form 


ôK = mr ôt + wlJ(óà — ba) + Sur Gad — Jóà)w (6.2.46) 
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After some manipulations we get the simplified expression 


óK = miT br + w J 5a (6.2.47) 
Similarly 
ôU = mg" dr (6.2.48) 
and 
OW = Fl br + NT ba (6.2.49) 
Therefore, owing to equation (6.2.1) 
t2 
J [mr or + w Jóà — mgl dr + F'ór + NTóo]dt = 0 (6.2.50) 
tı 
Let us integrate by parts the terms in ór and óà 
[uT J õa + miT ôr] is (6.2.51) 
t2 d 
+ if ions —mg+F) ot [N — qe) \ dt = 0 (6.2.52) 
ti 


Since the end-conditions are prescribed (ôr = 0 and ĝa = 0 in t = tı and t = t2), the 
boundary term in (6.2.51) vanishes. 


The arbitrariness of dr and ĝa provides the dynamic equilibrium equations in global coor- 


dinates: 
0.259 
and 


Equation (6.2.22) can be used to transform (6.2.54) into 
Jo + (#J—-—Jo)w = N (6.2.55) 


or 


Jo + GJu = N (6.2.56) 


To express translational equilibrium in body coordinates, the easiest is to premultiply equation 
(6.2.53) by RT, in which case one obtains the result 


a = Rig (6.2.58) 


where 


is the acceleration vector in body azes. 


For the rotational part, let us make use of the fact that 
Ka = w? J'(8à + Gol) (6.2.59) 
in which case 


t2 t2 
/ óKsdt = [6o Jw]? — f bal (J'à/ + à J'u/) dt (6.2.60) 
ii ti 


and rotational equilibrium expresses thus in the body axis form 


(621) 
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Figure 6.3.2: Geometry of binary link 


6.3 Recursive Newton-Euler technique for simple open- 
tree structures 
Let us consider member i of an open-tree simply connected articulated structure, and define 
- wi the angular velocity of link i expressed in the local frame T;; 
- vi the linear velocity of its center of mass in the local frame T;; 
- aj, its acceleration in the local frame T;; 


- ni, the torque exerced by link i on link 1+ 1, projected in frame T;,1; 


- n*,, the torque exerced by link i on link 7+ 1, projected in frame T;; 


- fj,, the force exerted by link i on link i+ 1, projected in frame T;+1; 


- fij, the force exerced by link i on link 7+ 1, projected in frame T;; 


- gi the location of the center of mass of link i measured from the origin of frame T;, and 
expressed in frame T7 


- d; the vector describing the length of member i, expressed in frameT;. ,. 
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According to this notation, the total external force acting on link i is 


Fi = -faut (6.3.1) 


t 


and similarly, the total external torque is 
N; = -nju +n; +g fi (d, +g) fi (6.3.2) 


According to (6.2.55) and (6.2.59), the equations of motion of link 7, expressed in body coordi- 
nates, are 

mai, = -mig + F; (6.3.3) 

Jie + Of Ful = Nl (6.3.4) 
Where g is the vector of the gravity acceleration. From the kinematics, we know that positions, 
velocities and accelerations can be obtained from a forward recursive procedure. Forces and 
torques, on the other hand, have to be determined backwards since the reaction forces at the 
manipulator base are unknown. The recursive Newton-Euler procedure is then as follows. 


6.3.1 Forward kinematic recursion 

initialization 
Gravity terms can be omitted from the procedure assuming that the ground has the 
acceleration of gravity, taken with negative sign 


wo = 0 Uy = 0 
wy = 0 a= -g (6.3.5) 


Where g is the gravity acceleration assumed to be oriented in the negative direction of zo 
axis. 


iteration i, (i — 1,...m) 
Velocities and accelerations are incremented and projected into the current body frame 


Wie = wct(ü-eoduak 
Gi = ù; + (-o)ldaid; + dii] k 
aj = a; + (ia — OE Gp)di. + o(2dj9; + dici)k 
TE m Ri, wip 
Witi = Ri, Uia 
aua = Rin aiy (6.3.6) 
The rotation operator R;,; and the length vector being extracted from the relative trans- 
formation 
beds ia E (6.3.7) 
for the center of mass, we simply have 
Bite = Q + Ria Cm - oia) 9i4i (6.3.8) 
where 
gii = Riy Iip (6.3.9) 


is the position of the center of mass of link i+ 1 expressed in frame T;. 
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6.3.2 Link inertia forces 
The link inertia forces are evaluated from (6.3.3) and (6.3.4) 


1 1 
Fiyi = Mitai, 
1 1 s anp 1 1 
N: L1 = J; 40i F Oj Jii Wip (6.3.10) 


a 


6.3.3 Force backward recursion 


The link interaction torques and forces are calculated from link n to link 1 according to the 
following scheme : 


Initialization 


The force F and torque C at the end of link n are those generated by the task 


you Ez RTF 
n;,,-— R'C (6.3.11) 
Iteration 
fi = Fi+ fin 
n; = Ni + nat Fi + difia 
fý = Ri fiy 
n; = Rin; (6.3.12) 


Note that the driving torque is the z component of torque n7. Therefore, 


Ti = k” n? (6.3.13) 


a 


6.4 Lagrangian dynamics 


Let (q1,q2,...q4) be the generalized coordinates which define completely the current configura- 
tion of the dynamic system. 


Let K and U be the kinetic energy and potential energy stored in the system, obtained by 
summing the contributions of individual links 


Kom WE. du cy. di (6.4.1) 


we define the Lagrangian of the total system by 
£L (gx, dk) = K-u (6.4.2) 


Note that, since the kinetic and potential energies are functions of q, and dy (k = 1,...n), so is 
the Lagrangian £. In terms of the Lagrangian, the equations of motion of the system are given 
by 

d ( OL ) OL 


rivo ce Qk bs sd (6.4.3) 
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where Qk are the generalized forces conjugated to the generalized coordinates qi. They can be 
obtained by expressing the virtual work of the non-conservative forces acting on the system and 
writing it in the form 


SW = M Qióq. (6.4.4) 
k=1 


6.4.1 Structure of the kinetic energy 


In the previous section, the kinetic energy of one single link has been put in the form 


1 D 1 1 
Ki = ji vi + gei Jii (6.4.5) 


when expressing linear and angular velocities in body axes. 


According to kinematics, in the absence of overall motion v; and w; can be expressed in the 
form of linear combinations of joint velocities 


v; = X ayla) dj 
j=1 
j=1 


where the coefficients a;; and bj; are vectors dependent on the current configuration. 
The total kinetic energy K stored in the whole structure is then given by 


TL 


K= MK = 2. maa) dj dk (6.4.7) 
i=1 j=1 k=1 


where, according to (6.4.5) and (6.4.6): 


e The coefficients mjg are configuration dependent and symmetric i.e. 
mjk(q) = mxj(q) (6.4.8) 


e The resulting mass matrix 
M = [my] 


is thus symmetric and also semi-positive definite since K > 0 for q Z 0. 


6.4.2 Potential energy 


In the absence of elastic deformation, the total potential energy of the system is made of the 
gravity contributions of the individual links 


u = Yu - S us (6.4.9) 
i=1 i=1 


Where ric is the position vector of the center of mass of link ? and g is the gravity acceleration 
vector. The potential energy is thus a function of configuration variables (q1,... qn). 
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6.4.3 Virtual work of external forces 


Let us consider the situation where actuators exert torques T = [ni,... Tn]? at individual joints 
and that the arm's endpoint, while it is in contact with the environment, exerts an external force 
and moment F. 


The virtual work is then given by 
W = t" ôq — FT ba (6.4.10) 


where x represents the tool configuration into task space. Making next use of the fact that the 
Jacobian matrix relates displacements increments into cartesian and joint spaces 


da = JÓq (6.4.11) 
equation (6.4.11) can be put in the form 
OW = ôq! (r — J’ F) (6.4.12) 


6.4.4 Structure of inertia forces 


Inertia forces can be expressed in the form 


d (OK) OK 
Exe Sin Al 
aaa) Gee sls 


According to (6.4.7), the first term of (6.4.13) can be computed as 


RE id 
a (mui) = » [mdi + L maj] (6.4.14) 
j=l j=l 


with 


d M Eum 


the second term of (6.4.13) is 


PIL d rdi (6.4.15) 


Oak 2 j 


OK 1c Y dm 
=1 [=1 
The result of (6.4.13) may thus be expressed in matrix form 


Màj -- Aq? (6.4.16) 


or alternatively 
Mä + C(q.d)d (6.4.17) 


where the second term accounts for centrigugal and Coriolis forces, and where the coefficient 
Ckj of matrix C are given by 


"Em = OMkj 1 mj : 
pe 2 EIU )à (6.4.18) 
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It is interesting to observe that the matrix 


S = M - 2C (6.4.19) 


has general term sgj which can be expressed in the form 


3 ( - ou 4 Fan) (6.4.20) 


so that it is anti-symmetric. This remark will be of importance for control purpose. 


6.4.5 Gravity forces 


Gravity forces produce generalized forces computed by 


Ou 2 pores 
Gk = E X mig A (6.4.21) 


j=l 


6.4.6 Equations of motion 


Collecting the different contributions to (6.4.3), we obtain the dynamic equilibrium equations 
in the general form 
Mä + A 4G J' Fr (6.4.22) 


6.5 Recursive Lagrangian formulation 


Just as Newton-Euler formulation, the Lagrangian formulation for the dynamics of an open-tree, 
simply-connected structure can be put in recursive form. 


To simplify the presentation, we will use homogeneous transformations to describe the kine- 
matics of the system. 


6.5.1 Forward kinematics 


Let us express that the position of an arbitrary point P on member i is obtained in homogeneous 
form by 
p = Tip (6.5.1) 


where p' is the position of same point P in the local frame T;. Transformation matrix T; is 
obtained from a forward recursion 


T; = 411A5...* 1 Aj (6.5.2) 
Similarly, velocities and accelerations are obtained by 
p = T p' (6.5.3) 


and ; 
p = T; p' (6.5.4) 


where T; and T; may also be computed recursively. Starting from 
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the first derivative is computed as follows 


T, = Ti. 1 Ai + Ti 1A; 


Or 
T; = [T1 +T; T'A] A (6.5.6) 
with » 
iiia. 5 |(1—-o)k ok 
Ai = di | oT 0 (6.5.7) 
Similarly, for the second derivative 
T; = T; AA; + 2T; ,À; + TÅ; 
Or 
(6.5.8) 
with SA Ar. 
i-1 x x 1—oc)k ck : 1—oc)kk' 0 
VAS = Gea | Ld 0 | - $a f 2 J (6.5.9) 
6.5.2 Kinetic energy 
The kinetic energy is the sum of individual link contributions 
Kom N G (6.5.10) 
i=1 
and the kinetic energy of one member is calculated by 
1 
Kis 7 pl pdm (6.5.11) 
Vi 
Note that it can be expressed in the equivalent form 
1 
Ki = J trace{p p^ ) dm (6.5.12) 
Vi 
in which case, in local coordinates 
1 ; : 
Ki = trace{t, | p p dm t} (6.5.13) 
Vi 
6.5.3 Matrix of inertia 
The matrix 
p= | pp dm (6.5.14) 
Vi 


represents again, but this time in homogeneous coordinates, the inertia properties of link 4 (ro- 
tational and translational properties). It is easy to control that with the definitions of moments 
of inertia 


Ten = [or iam Lp fy (z^ + 2)dm Lz = [a^ +? dm 
V ' Vi 


a 


Ly = n hz = m y' z'dm Ly = n 
Vi : Vi 
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and mass properties 


m= fy, dm 


mr, = f xam my, = [y ydm mz = f im 
Vi ' Vi 


inertia matrix J; can be has the explicit form 


(—Ine + Lyy + I,z)/2 Ley Teg ma, 
x. Tey (Iss — Iyy + I,)/2 Iyz my, 
qoe i. Tye Ui pese Oe) 
ma. my. mz, m 


Note that in the 4 x 4 representation, the fourth row and column of the matrix represents the 
translation inertia properties. 


6.5.4 Potential energy 


We consider only the potential energy due to gravity 


Ui = = f 9g. p dm (6.5.16) 
Vi 
or in terms of the position vector of the center of mass 
Ui = -mig Dic 


6.5.5 Partial derivatives of kinetic energy 


In order to express dynamic equilibrium, we have to calculate the inertia terms in the form 


d ,O0K OK 


aba) aq 6-1» (6.5.18) 


let us start by 


OK à [1 s ern 
T = xs 1; JT, 
Dan Dan E rasta JiT; }] 
= trace, —— JT; 6.5.19 
Yos (6.5.19) 
where we note that : 
OT; ; 
— = 0 if ki 
Odi. 
(6.5.20) 
OT; OT; 
= if k<i 
Od. Odk 


giving the result 


OK x OT, , ur 
Bx pony t } (6.5.21) 
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and thus 
d (een OT, " 
LJ; 7 6.5.22 
di Bin) Ti gg A uL 
Similarly, we have 
T 
~ = Dirala ITE} (6.5.23) 
and therefore we obtain the very compact result 
d a) = E 
— — L— t M E 6.5.24 
dt 55) K E race [S5 ? j ( ) 


which according to (6.5.8), can be computed recursively. 


6.5.6 Partial derivatives of potential energy 


The partial derivatives of potential energy are similarly expressed by 


Pic 


6.5.7 Equations of motion 


The equations of motion of the system are thus given by 


(6.5.26) 


In this form, making use of forward recursive procedure to compute T. the number of arithmetic 
operation has n? dependence. For n — 6, 705 multiplies and 5652 adds are required. 


6.5.8 Recursive form of equations of motion 


Let us note that 


OT; OT), 
= =? 6.5.27 
O4k O”k l ) 
in which case, the generalized forces (6.5.26) can be expanded as 
Z 9T, - aT, 
trace{ — "T; J; T — m; g? — PT; | = t 6.5.28 
Y | rracet )-mg wi TP) = Qu) (6.5.28) 


i=k 


To find a recursive computation procedure, one has to recast generalized forces in joints as: 


trace( E [So "T. J; qu — g" OT D m; "T; pie] = Q(t) (6.5.29) 
he eee k 4 
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Let us define 
Dit) = 3, "n ITT 
i=k 


= Jk T + 5 Apy EYT, Ji TE 
i=k+1 


or 
D, = J TE + Arpi Devt (6.5.30) 
Similarly, 
i=k 
or 


(53) 


Computations of Dx and C; lend themselves to a recursive backward computation procedure. 
These two quantities in hands, the generalized forces Qg developed in joints (6.5.29) take then 
the very simple form: 


Q(t) = pa uk Di) — a? 2f. (6.5.32) 
Odk qk 


In this backward recursive form, the number of operations has order n dependence. For n = 6, 
there are only 4388 multiplies and 3586 adds. 


There would still be a 50% improvement by omitting the 4 x 4 representation. 
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Chapter 7 


TRAJECTORY GENERATION 


CHAPTER 7. TRAJECTORY GENERATION 


Appendix A 


ELEMENTS OF VECTOR AND 
MATRIX ALGEBRA 


2 APPENDIX A. ELEMENTS OF VECTOR AND MATRIX ALGEBRA 


my 


Figure A.2.1: Geometric representation of a vector 


A.1 Introduction 
Kinematics and dynamic analysis of mechanical systems such as articulated mechanisms and 
robots implies using the appropriate tools for representing the geometry of the motion. 


Vector algebra is the appropriate tool to this end. Two different points of view may however 
be considered, according to the goal pursued. In the geometric approach, the basic operations 
of vector calculus are described in an intrinsic manner, i.e. vectors are “geometric beings” 
independent of the reference frame in which they are expressed. 


In the matrix approach, the basic operations of vector calculus are defined in an extrinsic manner, 
using algebraic quantities that describe the vectors in the reference frame under consideration. 


Most textbooks of dynamics use the geometric approach, which is the classical way of describing 
the kinematics of 3-dimensional motion. 


Here, a different point of view will be adopted. It is found that the matrix approach leads to a 
formalism which is much more appropriate for subsequent computer implementation. 


We will thus briefly recall the geometric concept of vector and the basic operations between 
vectors. We will next translate them in terms of matrix algebra. 


A.2 Definitions and basic operations of vector calculus 


Scalars 


In various physical applications there appear certain quantities, such as temperature, the specific 
mass of a material or the pressure in a fluid, which possess only magnitude. These can be 
represented by real numbers and are called scalars. 


Vectors 


On the other hand, a vector is a geometric quantity which possesses both magnitude and di- 
rection. Its geometric notation is the arrow symbol. Three types of vectors can be formally 
distinguished: 


- A free vector is a vector for which magnitude and orientation are specified, but not its line 
of action. 


- A bound vector is a vector issued from a specified point in space. 


- A sliding vector is a vector acting along a specified line in space (called its line of action), 
but at an arbitrary point of application. 


For example: 
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—» 
C 
b 
a 


Figure A.2.2: Geometric representation of the sum of two vectors 


- The position vector p of a given point P in a reference frame centered at O is a bound vector; 


- The force vector f representing the external force acting on a rigid body acts along a specified 
line of action and is thus a sliding vector; 


- The displacement vector d representing the displacement of a material point from A to B is 
a free vector. 


The magnitude or norm of a vector d is its length, which we denote by |a], or a. 


A unit vector, i.e. a vector with a norm equal to 1, specifies a direction. For example, we will 
denote t, the unit vector having same direction as d. 


We begin by considering the two following operations on vectors. 


Scalar multiplication 


The product of a vector d by a real scalar a is obtained by multiplying the magnitude of @ by 
o and retaining the same direction if a > 0 or the opposite direction if a < 0. 


'The scalar multiplication is distributive: 


(a + B) d = ad + Bd (A.2.1) 
Addition of two vectors 


The addition of two vectors d and b is governed by the so-called parallelogram rule, i.e. the 
resulting vector 


> 


€=a+b (A.2.2) 
is the diagonal of the parallelogram formed by @ and b as shown on figureA.2. 


The addition is commutative since (see figure A.2.3) 


@+b=b+4 (A.2.3) 


@+b+@=(@+b0+4@=4+(64+9 (A.2.4) 
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Figure A.2.3: Commutativity of the sum of two vectors 


Figure A.2.4: Associativity of the vector sum 


Figure A.2.5: Decomposition of a vector into cartesian components 
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Figure A.2.7: Dot product of two vectors 


Cartesian components of a vector 


Let us consider (figure A.2.5) a cartesian frame OXY Z with reference axes defined by the unit 
vectors Uz, Uy and tz. 


An arbitrary vector @ may be decomposed in a sum of three vectors with respective lengths az, 
ay and a, along the three reference axes. Using vector notation 


G = ay Uz + Qy Uy + az Uz (A.2.5) 


Let us denote next by 62, 6, and 0, the angles between the vector d and the coordinate axes. 
The components of d are then given by 


ax = a cos0, = a by 
dy = a cosy = a£, (A.2.6) 
à; = a cos0, = ai, 


where lz, ly and £; stand for the direction cosines of d. 


Dot product of two vectors 


The dot or scalar product of two vectors d and b is defined as the scalar resulting from the 
product of the norm of these vectors and the cosine of their relative angle (see figure A.2.7) 


deb = ab cos (A.2.7) 


the 0 angle being measured in the intersecting plane of both vectors. 


Immediate consequences of this definition are that 
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ey 


Figure A.2.8: Cross product of two vectors 


- The dot product of two orthogonal vectors is zero; In particular, the unit vectors along the 
reference axes form a normal basis such that 


eerste Se "E 
Uz@Uy = üyeü; = ü;euü, = 0 
Uz èür = tyety = Ü, oü, = 1 (A.2.8) 


- The dot product of a vector d by itself is equal to the square of its norm 


ded = a? (A.2.9) 


- The dot product is commutative 7 
Geb = beg (A.2.10) 


- In terms of its cartesian components, the dot product d e b reads 


deb = agily + ayüy + a,ü,)e(b,ü, + by, + beitz) 
= ayby + ayby + a£. (A.2.11) 


since the unit vectors tz, i4, and à; form an orthonormal basis; 
- 'The dot product is distributive 


de(b + @) = deb + dec (A.2.12) 


- The angle between the two vectors may be found from their dot product by 


cos = deb _ arbe + ayby + azbz 
ab ab 
= lyme + lymy + £m; (A.2.13) 


where the m; stand for the direction cosines of vector b. 


Cross or vector product of two vectors 


The cross product of two vectors d and b (see figure A.2.8) is defined as a vector whose magnitude 
equals the product of the magnitudes of à and b multiplied by the sine of their relative angle. 
Its direction, denoted by the unit vector wu, is normal to the plane defined by the right-hand 
screw rule. The cross product may thus be written 


axb = ab sinb d (A.2.14) 


Let us note that according to this definition 
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- Reversing the order of the cross product would reverse the sign of the vector ü. Hence, the 
cross product is not commutative: 


@xb = — bxà (A.2.15) 


- The cross product of two parallel vectors is zero. In particular, 


ax@ = 0 (A.2.16) 


- The orthogonal vectors defining the cartesian frame are such that 


S2 res zv d 5 
Ug X Uy = — UyXUp = Uz 
Pee A. - 
Uy XU = — Uz; X Uy = Uy 
Üz X Ür = —ty Xtz = ty (A.2.17) 


- In terms of cartesian coordinates, the dot product has for expression 


dxb = (ay, + ayü,  a,ü,) x (brit, + byü, + brit.) 
= (ayb, — a;b,)ü, + (a,b, — Gzbz)tly 
+ (az;b, — aybz)uz (A.2.18) 


- From the relation above, it is easy to control that the dot product is distributive 


= 


(@+b)x@= áxe& 4 bxe& (A.2.19) 


Additional relationships 


'Two additional relations of vector algebra are given without proof, but their validity is easy to 
demonstrate by decomposing them into cartesian components. 


'The triple scalar product is the dot product of two vectors where one of them is specified as 
a cross product of two additional vectors. The result is a scalar and is given by one of the 
equivalent expressions obtained by cyclic permutation 


(qx bled = (bx Qed = (Exod (A.2.20) 


It may also be seen upon expansion that 


E 


(@x b)eZ = dtm | b, b, b (A.2.21) 


which means that its absolute value is the volume of the parallelipiped with sides a, b and Z. 


'The triple vector product is the cross product of two vectors where one of them is specified as 
a cross product of two additional vectors. The result is a vector and is given by one of the 
equivalent expressions 


(@xb)x@ = — &ex(üxb) = Ex (bx @) (A.2.22) 
It is easily shown that it is equivalent to 


=> > 


(axb)xc- 


| 
PG 
Q 
e 
S 
e 
| 
vet 
e 
e 
S 
Q 


(A.2.23) 


and similarly " 7 
ax (bx ce) = (dec)b — (deb).c (A.2.24) 
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A.3 Definitions and basic operations of matrix algebra 


By definition, a rectangular matrix A of dimension m x n is the rectangular array made of m 
rows and n columns 


Q11 Q12 «+s Qin 
a21 22  ...  Q2n 

A = [ay] = ; a. ; (A.3.1) 
Am1 Am2 3. Amn 


where its general element a;;, a scalar, is located at the intersection of row i and column j. To 
note it, we use boldface capital letters. 


Transpose of a matrix 


The transpose of matrix a, denoted by A’, is the rectangular array of dimension n x m obtained 
by interverting the rows and columns of the original matrix. 


Column- and row-matrices 


A matrix made of only one column is a column matrix; it will be denoted by using boldface 
small letters. 


a= . (A.3.2) 


Similarly, a matrix made of only one row is a row matrix. It may be written as the transpose 
of a column matrix 


al = [ a, ag ...Óg ] (A.3.3) 
Null matrix 
A null matrix is a rectangular matrix made of zeros 
A= 0 is such that aj = 0 i—l...m, j=1,...m (A.3.4) 


Square matrix 


A square matrix is a matrix having equal number of rows and columns. Furthermore: 


- It is symmetric, if two terms located symmetrically with respect to the diagonal are equal 


Qij = Qji for all i,j =1,...n (A.3.5) 


- It is skew-symmetric, or antisymmetric, if the sum of two terms located symmetrically with 
respect to the diagonal is zero 


di; + aj —0 for all i,j =1,...n (A.3.6) 
A consequence of this definition is that a skew-symmetric matrix has zero diagonal terms 


an = 0 forall t=1,...n (A.3.7) 
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- A square matrix is diagonal if only the diagonal terms do not vanish 


Qjj = 0 for all izj (A.3.8) 
in which case it may also be represented by 
A= diag [ Q11 422 0 Ann ] (A.3.9) 
- It is quasi-diagonal if it is made of square matrices B1, Bo, ... B, 
Bi (0) 
B2 

A= . (A.3.10) 

o B, 


- The unit or identity matrix of dimension n x n is the square diagonal matrix made of diagonal 
terms equal to 1 


I = diag |1 1 22s 1] (A.3.11) 
Equality of two matrices 


Two matrices A and B are said to be equal if they have the same dimension and if the corre- 
sponding elements are equal: 


aj = bg  (i=1,...m, j=1,...n) (A.3.12) 


Addition and difference of two matrices 


The addition or sum of two matrices A and B having the same dimension is defined by the 
operation 


C= A+B (A.3.13) 
where the general term of C is given by 
Cy = aij + bij @Hlya omy fH Taser) (A.3.14) 
Similarly, the difference of two matrices A and B of same dimension is defined by the operation 
C— A- B (A.3.15) 
with 
Cijj = aij — bij G= lenm, J= lran) (A.3.16) 
According to the definition, matrix addition is commutative 
A+B=B+iA (A.3.17) 
It is also associative 
(A+ B)+C=A4+ (B+ CO) =AHBHC (A.3.18) 
The transpose of the sum of two matrices is equal to the sum of the transposed matrices 
(A + B)? = AT + BT (A.3.19) 


Finally, any square matrix may be expressed as the sum of a symmetric and a skew-symmetric 
matrices 
A=B++C (A.3.20) 
with i i 
B= (A AT) ad C= zA- AT) (A.3.21) 
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Scalar multiplication of a matrix 


The multiplication of a matrix .A by a scalar o (a scalar being always, with our notation, denoted 
by a small Greek letter) 
aA = C (A.3.22) 


where 
Cig = Qij ((i—1,...m, j-1l,..m) (A.3.23) 
Matrix multiplication 


Let A and B be two matrices of dimensions m x p and p x n respectively, which we write in 
the form 


T 
where the a? (i=1,...m) and b; (j =1,...n) are row- and column-matrices of dimension 
p. 

One defines first the dot product a7 b of a row matrix with a column matrix 


alb = aıbı + azb2 Fay Qm Uy, 
p 
= y aibi (A.3.25) 
i=1 
The matrix product of A and B is defined next as the matrix C of dimension m x n 
C— AB (A.3.26) 
such that 
Cy = a; b; (A.3.27) 
or, in terms of the elements of A and B 
p 
Cj = ` Qikbkj (A.3.28) 
k=1 


It is fundamental noticing that a matrix multiplication can be performed only if the number of 
columns of the first matrix equals the number of rows of the second one. 


Let us also note that, from its very definition, 
- The operation of matrix multiplication is non-commutative 
ABACA (A.3.29) 


- It is distributive, since for A and B of dimension m x p and C of dimension p x n one can 
write 


(A+ B)C=AC+ BC (A.3.30) 
- It is associative, since for A, B and C of dimension m x p, p x q and q x n respectively 
(A B)C = A(BC) - ABC (A.3.31) 
- The transpose of a matrix product is the product of the transposes taken in reverse order 


(A B)! = BT AT (A.3.32) 
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Rank of a matrix 


Let us consider a matrix A of dimension m x n, decomposed in terms of its columns 
A = [a a ... an] (A.3.33) 


Its columns are said to be linearly independent if any linear combination of these does not 
vanish. In other terms, the columns of a are linearly independent if, for b arbitrary 


bT = [bi bo... bn] (A.3.34) 
one has always 
Ab#0 (A.3.35) 


If (A.3.35) does not necessarily hold, then the columns of A are linearly dependent and one of 
them at least may be expressed as a linear combination of the others. 


The column rank (or the row rank) of a matrix A is defined as the largest number of linearly 
independent columns (or rows) that can be extracted from A. 

It is possible to show that the row rank and the column rank of a matrix are equal. Therefore, 
we define the rank of matrix A as the common value of its row and column ranks. 


The rank of matrix A is also equal to the dimension of the largest square submatrix with non- 
zero determinant that can be extracted from A through appropriate deletion of rows and/or 
columns. 


A square matrix A having linearly independent rows and columns is said to have maximum 
rank. Conversely, if it has not maximum rank, it is said singular. 


A square matrix A having maximum rank is also said nonsingular. 


Inverse of a matrix 


A non singular square matrix A possesses an inverse, denoted AT}, such that 
AA't=TI (A.3.36) 


where I is the unit matrix. Its explicit expression can be obtained from Cramer’s rule for the 
solution of linear systems of equations, and textbooks on numerical analysis present various 
techniques for calculating it numerically. 


It is easy to show that 
- The inverse of A is the transpose of the inverse 
(AS) = (AT)! = ATT (A.3.37) 
Orthogonal matrix 


A non-singular matrix which plays a fundamental role to describe the kinematics of a rigid body 
is the orthogonal matrix. It is such that 


A^! = ATJqdet(A) (A.3.38) 
where det(A) stands for the determinant of A. If det(A) = 1, then A is orthonormal: 
A = AT (A.3.39) 


As computing the inverse of a nonsingular matrix is a costly operation, it is important to know 
whether a matrix is orthonormal or not. 


Let us further note that in most cases, a matrix is called orthogonal even if it is also orthonormal. 
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A.4 Matrix representation of vector operations 
The matrix representation of vectors provides, as it will be seen, a powerful formalism for 
performing the main operations of vector calculus. 


Once a specific cartesian reference frame is adopted in which vectors are represented in an 
extrinsic manner, any vector may be represented by a column matrix collecting its cartesian 
components in this frame. 


Let @ and b two vectors which we represent in matrix form by 
a’ = [ Az Gy Gz ] (A.4.1) 
b” = [bs by b] (A.4.2) 


Vector sum 


The vector sum Z = d + b is equivalent to the matrix operation 


c=a+b (A.4.3) 
Equality of two vectors 
The equality d = b holds if and only if d and b have the same cartesian components in the 
same frame. Thus, 
a = b (A.4.4) 


Scalar multiplication 


The product of à by a scalar o, which gives the vector od, has the matrix representation 
c — aa (A.4.5) 
Dot product 
The dot product d e b of two vectors d and b can be written in matrix form 
Tqcueqme 
a b = b'a = ayb4 + ayby + azbz (A.4.6) 


Cross product 


Let us define the skew-symmetric matrix associated to a vector d, such that 


0 —üz Ay 
a= az 0  —a, (A.4.7) 
—üy Ay 0 


The properties of the skew-symmetric matrix à are the following: 


- Its transpose is such that 
al = —à (A.4.8) 


- The scalar multiplication is such that 
aà = aa (A.4.9) 
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- The vanishing of the cross-product of a vector by itself leads to 


and . 
ab 


so that 
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(A.4.10) 


(A.4.11) 


(A.4.12) 


(A.4.13) 
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ELEMENTS OF QUATERNION 
ALGEBRA 
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B.1 Definition 
A quaternion is defined a a 4-dimensional complex number 
d = qo + iq + jq» + kas (B.1.1) 


with i, j, k being imaginary unit numbers such that 


Pag ak = at 

jk = —kj =i 

ki = —ik =j 

ij = —ij = (B.1.2) 


It can be alternatively written in vector notation 
å= do - d— qo * 4 (B.1.3) 

where qo is the scalar part of the quaternion g while d or q are its vector part. 

The multiplication rule is a direct consequence of the definition (B.1.1). In algebraic form, 
the resulting quaternion can be written: 

Pd = (po+ipı + jpa t kps) (qo + iqı + jq2 + kgs) 
= (pogo — p»141 — P202 — P343) 

+ i (pogi + P190 + P293 — paq2) 


+ j (poqa + p2do + p3qı — p143) 
+ k (poda + p3qo + p192 — p2d1) (B.1.4) 


In vector form the quaternion product is given by 


PG = (pot+P) (a+ q) 
= podqo—P-:d ^ pod ^ doPp - pPxdqd 
= podqo—-p!q + po q ^ do p * pag (B.1.5) 


Because of the presence of the cross product in definition (B.1.5) the product is an associative 
but non-commutative operation. 


The conjugate quaternion to ĝ is defined as 
G = — iq — ja — kas 
d — q (B.1.6) 
It is easily verified that the conjugate of a quaternion product is such that 
(p â) =e (B.1.7) 
The norm of a quaternion is calculated by 


lal? = Ga — a; - dd (B.1.8) 


In particular, d is a unit quaternion if 


llall = 1 (B.1.9) 
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A quaternion ĝ is a vector quaternion if 
G =0+¢ (B.1.10) 


in which case 
G+¢ =0 (B.1.11) 


Vector quaternions describe position vectors, linear and angular velocity vectors, etc. 


B.2 representation of finite rotations in terms of quater- 
nions 


Given a unit quaternion ê = eg + € and the position vector ĉ = 0+ z, the finite rotation of x to 
a new position j/ may be represented by the triple quaternion product 


y= êĉ ê (B.2.1) 
The proof holds by noting that 
e ¥ is also a vector quaternion 
g+ 9" =0 
e the length of ĉ is conserved 
all? = 8$* = Ila)? 


The inverse rotation is directly obtained in terms of the conjugate quaternion 


é (B.2.2) 


A, A* 
t =e 


e 


Let the position vector undergo two successive rotations 


2 = 694605 = (6261) $ (62€1)* (B.2.3) 
'The resulting rotation is given by 
2 = êĉê* with ê = 636 (B.2.4) 
Clearly every unit quaternion can be expressed in the form 
ê = cosa + n sing (B.2.5) 
where n a unit vector. 
If we perform the operations indicated in (B.2.2), we obtain: 
é$ = -sina nlg + cssax + sina ñ æ (B.2.6) 
The vector character of the result is restored after performing the ’symmetric’ operation: 


j= eae 


= sin?a(n-a) + cos*ax + 2 sina cosa f a — sin?o (f x) x 


Which provides after developing 
T 


g = 0 + (cos2a I + (1—cos20) n m^. + sin2an) « (B.2.7) 


Comparison with expression of rotation matrix in terms of Euler parameters shows this operation 
is a rotation of angle 2a about n. This demonstrates that there is an equivalence between Euler 
parameters and a double quaternion products. 
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B.3 Matrix representation of quaternions 


A quaternion may be represented in matrix form by 4-dimensional column matrix 


å = [mu e q3 |" 
= [do a]* (B.3.1) 
in which case the quaternion product à = f ĝ can be written in either form 
a= Ap = Bj (B.3.2) 
with the 4 x 4 matrices 
s-[rams] [fam] oe 


where T is the unit matrix and q is a skew-symmetric matrix attached to the vector part q 


0 -%8 qa 
q = qa 0 —(1 (B.3.4) 
—q2 q 0 


B.4 Matrix form of finite rotations 
By using (B.3.2), the rotation operator can recast in the form 
ĝ = ABTĉ (B.4.1) 


When computing the matrix product A BT one finds 


ABT = | : d | (B.4.2) 


where the 3 x 3 submatrix R is the standard rotation operator. Developing the product (B.4.2) 
provides the classical result 


R = (2€ —1) + 2(ee' + eé) (B.4.3) 


where eo, e are the components of the unit quaternion é. 


A simplified form of R is given by 
R= EG (B.4.4) 


with 
E={[-e eIl+é |] and G = |-e elI-é | (B.4.5) 
where E and G verify the relationships: 
EE’ = GG? =I 
ETE = G7G = I — éé" 
Eê = Gê=0 (B.4.6) 
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B.5 Angular velocities in terms of quaternions 


Let us start from the quaternion form of a finite rotation 
ĵ = êĉ ê” (B.5.1) 


where ĉ is a vector quaternion describing the position of a point on a rigid body, and ê is a unit 
quaternion (Euler parameters). If the length of ? is assumed constant with time and that only 
ê is time dependent, then the following equation describes spherical motion: 


g(t) = e(t) & e*(t) (B.5.2) 
The associated velocity field is ' 
ĝ = êĉ ê + 64% (B.5.3) 


or in terms of ĝ 


à—à&$ 4 ge (B.5.4) 
If one notes further that éé* = 1 generates the identity 
eë + ée =0 (B.5.5) 
one deduces that the quaternion . 
@ = 268 (B.5.6) 
is also of vector type, and using the definition (B.5.6) equation (B.5.4) becomes 
n los uk 
es (og — dc) (B.5.7) 


It represents the velocity vector of P in terms of quantities in the frame attached to j. Equations 
(B.5.1) and (B.5.7) are thus analog to the matrix expressions 


y = Re and gj - RR” y (B.5.8) 
meaning thus that the vector quaternion w represents the angular velocity matrix 
© = RR (B.5.9) 


Similarly, velocities can be calculated in the frame attached to ĉ 


ô = ê& jê (B.5.10) 
or, making use of the equation (B.5.3) 
v = &éR + & OT (B.5.11) 
If one defines thus the quaternion vector 
a = 2é*é (B.5.12) 


velocities in the frame attached to ? may be written in the form 
6 = =(0'& — 40’) (B.5.13) 


Equation (B.5.13) is thus the quaternion analog of 
a = RR (B.5.14) 
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B.6 Matrix form of angular velocities 


The matrix notation introduced in sections B.3 and B.4 allows us to rewrite (B.5.6) and (B.5.12) 
in the matrix forms 


® = 2 B(ê)ê and à?' = 2A(é*)é (B.6.1) 
Or if we limit ourselves to the vector part of à and &’, one obtains the simplified expressions 
w —2Eé and w = 2Geé (B.6.2) 
where E and G are 3 x 4 matrices extracted from A and B 
E = [-e eI +é] and G = [-e eoI - €] (B.6.3) 


Explicitly, in terms off Euler parameters one gets 


w= 2 —€2 €3 €o —€1 [ ĉo ĉi È2 €3 ] (B.6.4) 


B.7 Examples 


B.7.1 Example 1: Composition of rotations with quaternions 


Find the equivalent angle-axis form of R(I,¢) to a rotation R(z,90°) followed by a rotation 
R(y, 90°). 


Using quaternion notations, one has 


with 


61 = (cos45° + k sin 45?)65 = (cos45? + j sin45?) 


So one gets 


ê= cos? 45° + (j +k) cos45° sin 45? + (j x k) sin? 45° 
V3 


1 
cos60° + — (i + j + k) sin 60? 


v3 


Therefore the overall transformation is equivalent to a rotation 


Il 


R(n, 120?) with n = ($4 j 4- k) 


ET 
v3 
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Rotated 
coordinate 
system 


Reference 
coordinate 
x system 


Figure B.7.1: The rotation of a coordinate system is described by a quaternion 


B.7.2 Example 2: Using quaternions for determining robot orienta- 
tion 


Useful formula 


The orientation of a coordinate system (such as that of a tool) can be described by a rotational 
matrix that gives the directions of the axes of the coordinate system in relation to a reference 
system (see Fig. B.7.1) The rotated system axes (x, y, z) are vectors which can be expressed 
in the reference system as follows: 


zt = (xı, T2, x3) 
y = (y1, Y2, Y3) 


z= (z1, 22; 23) 


This means that the x-component of the x-vector in the reference coordinate system will be z, 
the y-component will be 22, etc. 
'These three vectors can be put together in a matrix, a rotational matrix, where each of the 
vectors form one of the columns: 
Xi yi 441 
r2 Y2 272 
T3 Y3 23 


A quaternion is a more concise way to describe this rotational matrix; one can calculate the 
quaternion from the rotational matrix: 


31-c yo» 2341 
qo — = AA acr e 
2 
qı = MEM with signqi = sign (ys — 22) 
Zz- z +l 
q = — with signq2 = sign (zı — x3) 
ds = — with signgs = sign (xs — yi) 


Moreover these four quantities are nor independent and linked by the relationship 


g@t+agt+aeat+g=1 
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Figure B.7.2: The orientation of the wrist with respect to the base frame 


Orientation of the wrist with respect to the base frame 


A tool is oriented so that its z/-axis points straight ahead (in the same direction as the x-axis 
of the base coordinate system). The y'-axis of the tool corresponds to the y-axis of the base 
coordinate system (see Fig. B.7.2). How is the orientation of the tool defined in the position 
data? 

The axes will then be related as followed: 


vz’ ——z -—(0,0,-1) 
y =y =(0,1,0) 
E dw =(1,0,0) 
Which corresponds to the following rotational matrix: 
0 0 1 
0 1 0 
-1.0 0 


'The rotation matrix provides the corresponding quaternion: 


0+1+041 V2 
Dae ge Eit 
0—1—0-241 
pi = -zy = 0 
1—-0—0-1 2 
n a D = 0,707 with signgo = sign(1—(—1)) = + 
0—0—1241 
py = MM = 0 


Orientation of the tool with respect to the wrist frame 


The direction of the tool is rotated about 30? about the a’ and the z'-axes in relation to the 
wrist coordinate system (see Fig. B.7.3). How is the orientation of the tool defined with respect 
to the wrist system ? 


B.7. EXAMPLES 


Which corresponds to the following rotational matrix: 


qo 


q1 


q2 


q3 


Figure B.7.3: The orientation of the tool with respect to the wrist frame 


The axes will then be related as followed: 


The rotation matrix provides the corresponding quaternion: 


cos 30° + 1 + cos 30° 4 


2 
cos 30° — 1 — cos 30° 4 

2 
= 1 — cos 30° — cos 30° 4 

E 2 
cos 30? — cos 30? — 1 4 

2 


cos 30? 


0 


— sin 30? 


0, 9659 


0, 2588 


x” = (cos30°, 0, — sin 30°) 
y" = (0, 1, 0) 
z” = (sin 30°, 0, cos 30°) 


sin 30° 


with 


0 cos30° 


sign qo 


Orientation of the tool with respect to the base frame 


sign (sin 30° — (— sin 30°)) = + 


Orientation of the tool system with respect to base frame system is given by undergoing the 
two elementary frame transformations. In quaternion notations the global transformation is 
described by the following quaternion f: 


Q> 
D> 
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